<!DOCTYPE html PUBLIC "-//W3C//DTD XHTML 1.0 Transitional//EN" "http://www.w3.org/TR/xhtml1/DTD/xhtml1-transitional.dtd">
<html xmlns="http://www.w3.org/1999/xhtml">
<head>
<meta http-equiv="Content-Type" content="text/xhtml;charset=UTF-8"/>
<meta http-equiv="X-UA-Compatible" content="IE=9"/>
<meta name="generator" content="Doxygen 1.8.11"/>
<title>Cheetah Software: FloatingBaseModel.cpp Source File</title>
<link href="../../tabs.css" rel="stylesheet" type="text/css"/>
<script type="text/javascript" src="../../jquery.js"></script>
<script type="text/javascript" src="../../dynsections.js"></script>
<link href="../../navtree.css" rel="stylesheet" type="text/css"/>
<script type="text/javascript" src="../../resize.js"></script>
<script type="text/javascript" src="../../navtreedata.js"></script>
<script type="text/javascript" src="../../navtree.js"></script>
<script type="text/javascript">
  $(document).ready(initResizable);
  $(window).load(resizeHeight);
</script>
<link href="../../search/search.css" rel="stylesheet" type="text/css"/>
<script type="text/javascript" src="../../search/searchdata.js"></script>
<script type="text/javascript" src="../../search/search.js"></script>
<script type="text/javascript">
  $(document).ready(function() { init_search(); });
</script>
<link href="../../doxygen.css" rel="stylesheet" type="text/css" />
</head>
<body>
<div id="top"><!-- do not remove this div, it is closed by doxygen! -->
<div id="titlearea">
<table cellspacing="0" cellpadding="0">
 <tbody>
 <tr style="height: 56px;">
  <td id="projectalign" style="padding-left: 0.5em;">
   <div id="projectname">Cheetah Software
   &#160;<span id="projectnumber">1.0</span>
   </div>
  </td>
 </tr>
 </tbody>
</table>
</div>
<!-- end header part -->
<!-- Generated by Doxygen 1.8.11 -->
<script type="text/javascript">
var searchBox = new SearchBox("searchBox", "../../search",false,'Search');
</script>
  <div id="navrow1" class="tabs">
    <ul class="tablist">
      <li><a href="../../index.html"><span>Main&#160;Page</span></a></li>
      <li><a href="../../namespaces.html"><span>Namespaces</span></a></li>
      <li><a href="../../annotated.html"><span>Classes</span></a></li>
      <li class="current"><a href="../../files.html"><span>Files</span></a></li>
      <li>
        <div id="MSearchBox" class="MSearchBoxInactive">
        <span class="left">
          <img id="MSearchSelect" src="../../search/mag_sel.png"
               onmouseover="return searchBox.OnSearchSelectShow()"
               onmouseout="return searchBox.OnSearchSelectHide()"
               alt=""/>
          <input type="text" id="MSearchField" value="Search" accesskey="S"
               onfocus="searchBox.OnSearchFieldFocus(true)" 
               onblur="searchBox.OnSearchFieldFocus(false)" 
               onkeyup="searchBox.OnSearchFieldChange(event)"/>
          </span><span class="right">
            <a id="MSearchClose" href="javascript:searchBox.CloseResultsWindow()"><img id="MSearchCloseImg" border="0" src="../../search/close.png" alt=""/></a>
          </span>
        </div>
      </li>
    </ul>
  </div>
  <div id="navrow2" class="tabs2">
    <ul class="tablist">
      <li><a href="../../files.html"><span>File&#160;List</span></a></li>
      <li><a href="../../globals.html"><span>File&#160;Members</span></a></li>
    </ul>
  </div>
</div><!-- top -->
<div id="side-nav" class="ui-resizable side-nav-resizable">
  <div id="nav-tree">
    <div id="nav-tree-contents">
      <div id="nav-sync" class="sync"></div>
    </div>
  </div>
  <div id="splitbar" style="-moz-user-select:none;" 
       class="ui-resizable-handle">
  </div>
</div>
<script type="text/javascript">
$(document).ready(function(){initNavTree('da/de7/_floating_base_model_8cpp_source.html','../../');});
</script>
<div id="doc-content">
<!-- window showing the filter options -->
<div id="MSearchSelectWindow"
     onmouseover="return searchBox.OnSearchSelectShow()"
     onmouseout="return searchBox.OnSearchSelectHide()"
     onkeydown="return searchBox.OnSearchSelectKey(event)">
</div>

<!-- iframe showing the search results (closed by default) -->
<div id="MSearchResultsWindow">
<iframe src="javascript:void(0)" frameborder="0" 
        name="MSearchResults" id="MSearchResults">
</iframe>
</div>

<div class="header">
  <div class="headertitle">
<div class="title">FloatingBaseModel.cpp</div>  </div>
</div><!--header-->
<div class="contents">
<a href="../../da/de7/_floating_base_model_8cpp.html">Go to the documentation of this file.</a><div class="fragment"><div class="line"><a name="l00001"></a><span class="lineno">    1</span>&#160;</div><div class="line"><a name="l00018"></a><span class="lineno">   18</span>&#160;<span class="preprocessor">#include &quot;<a class="code" href="../../d5/d9a/_floating_base_model_8h.html">Dynamics/FloatingBaseModel.h</a>&quot;</span></div><div class="line"><a name="l00019"></a><span class="lineno">   19</span>&#160;<span class="preprocessor">#include &quot;<a class="code" href="../../d4/dfb/orientation__tools_8h.html">Math/orientation_tools.h</a>&quot;</span></div><div class="line"><a name="l00020"></a><span class="lineno">   20</span>&#160;</div><div class="line"><a name="l00021"></a><span class="lineno">   21</span>&#160;<span class="preprocessor">#include &lt;<a class="code" href="../../d3/d68/_utilities__print_8h.html">Utilities/Utilities_print.h</a>&gt;</span></div><div class="line"><a name="l00022"></a><span class="lineno">   22</span>&#160;<span class="preprocessor">#include &lt;stdio.h&gt;</span></div><div class="line"><a name="l00023"></a><span class="lineno">   23</span>&#160;<span class="preprocessor">#include &lt;stdexcept&gt;</span></div><div class="line"><a name="l00024"></a><span class="lineno">   24</span>&#160;<span class="preprocessor">#include &lt;string&gt;</span></div><div class="line"><a name="l00025"></a><span class="lineno">   25</span>&#160;<span class="preprocessor">#include &lt;vector&gt;</span></div><div class="line"><a name="l00026"></a><span class="lineno">   26</span>&#160;</div><div class="line"><a name="l00027"></a><span class="lineno">   27</span>&#160;<span class="keyword">using namespace </span><a class="code" href="../../da/d9b/namespaceori.html">ori</a>;</div><div class="line"><a name="l00028"></a><span class="lineno">   28</span>&#160;<span class="keyword">using namespace </span><a class="code" href="../../de/d1a/namespacespatial.html">spatial</a>;</div><div class="line"><a name="l00029"></a><span class="lineno">   29</span>&#160;<span class="keyword">using namespace </span><a class="code" href="../../d8/dcc/namespacestd.html">std</a>;</div><div class="line"><a name="l00030"></a><span class="lineno">   30</span>&#160;</div><div class="line"><a name="l00039"></a><span class="lineno">   39</span>&#160;<span class="keyword">template</span> &lt;<span class="keyword">typename</span> T&gt;</div><div class="line"><a name="l00040"></a><span class="lineno"><a class="line" href="../../d6/db1/class_floating_base_model.html#a210aa49a2de27eae6f3a0a839413c3b2">   40</a></span>&#160;T <a class="code" href="../../d6/db1/class_floating_base_model.html#a1edbea0371cffe8692a83ae5a9396e41">FloatingBaseModel&lt;T&gt;::applyTestForce</a>(<span class="keyword">const</span> <span class="keywordtype">int</span> gc_index,</div><div class="line"><a name="l00041"></a><span class="lineno">   41</span>&#160;                                       <span class="keyword">const</span> <a class="code" href="../../d9/d8e/cpp_types_8h.html#a6f07728c866b15ff0676a9b2ebfd2ba6">Vec3&lt;T&gt;</a> &amp;force_ics_at_contact,</div><div class="line"><a name="l00042"></a><span class="lineno">   42</span>&#160;                                       <a class="code" href="../../d9/d8e/cpp_types_8h.html#ae2e5a0a178a2c528d8aa939f85db7a3e">DVec&lt;T&gt;</a> &amp;dstate_out) {</div><div class="line"><a name="l00043"></a><span class="lineno">   43</span>&#160;  forwardKinematics();</div><div class="line"><a name="l00044"></a><span class="lineno">   44</span>&#160;  updateArticulatedBodies();</div><div class="line"><a name="l00045"></a><span class="lineno">   45</span>&#160;  updateForcePropagators();</div><div class="line"><a name="l00046"></a><span class="lineno">   46</span>&#160;  udpateQddEffects();</div><div class="line"><a name="l00047"></a><span class="lineno">   47</span>&#160;</div><div class="line"><a name="l00048"></a><span class="lineno">   48</span>&#160;  <span class="keywordtype">size_t</span> i_opsp = _gcParent.at(gc_index);</div><div class="line"><a name="l00049"></a><span class="lineno">   49</span>&#160;  <span class="keywordtype">size_t</span> i = i_opsp;</div><div class="line"><a name="l00050"></a><span class="lineno">   50</span>&#160;</div><div class="line"><a name="l00051"></a><span class="lineno">   51</span>&#160;  dstate_out = <a class="code" href="../../d9/d8e/cpp_types_8h.html#ae2e5a0a178a2c528d8aa939f85db7a3e">DVec&lt;T&gt;::Zero</a>(_nDof);</div><div class="line"><a name="l00052"></a><span class="lineno">   52</span>&#160;</div><div class="line"><a name="l00053"></a><span class="lineno">   53</span>&#160;  <span class="comment">// Rotation to absolute coords</span></div><div class="line"><a name="l00054"></a><span class="lineno">   54</span>&#160;  <a class="code" href="../../d9/d8e/cpp_types_8h.html#aa8cb424e643d476ea485238dad7c3e18">Mat3&lt;T&gt;</a> Rai = _Xa[i].template block&lt;3, 3&gt;(0, 0).transpose();</div><div class="line"><a name="l00055"></a><span class="lineno">   55</span>&#160;  <a class="code" href="../../d9/d8e/cpp_types_8h.html#ae8ab6bd4079a5317297be5eeb331b1ca">Mat6&lt;T&gt;</a> Xc = <a class="code" href="../../de/d1a/namespacespatial.html#ae4a2145f3028087511b4f17c0152ec83">createSXform</a>(Rai, _gcLocation.at(gc_index));</div><div class="line"><a name="l00056"></a><span class="lineno">   56</span>&#160;</div><div class="line"><a name="l00057"></a><span class="lineno">   57</span>&#160;  <span class="comment">// D is one column of an extended force propagator matrix (See Wensing, 2012</span></div><div class="line"><a name="l00058"></a><span class="lineno">   58</span>&#160;  <span class="comment">// ICRA)</span></div><div class="line"><a name="l00059"></a><span class="lineno">   59</span>&#160;  <a class="code" href="../../d9/d8e/cpp_types_8h.html#a3d70f8eb9cf1855ca989f887a5543acc">SVec&lt;T&gt;</a> F = Xc.transpose().template rightCols&lt;3&gt;() * force_ics_at_contact;</div><div class="line"><a name="l00060"></a><span class="lineno">   60</span>&#160;</div><div class="line"><a name="l00061"></a><span class="lineno">   61</span>&#160;  T LambdaInv = 0;</div><div class="line"><a name="l00062"></a><span class="lineno">   62</span>&#160;  T tmp = 0;</div><div class="line"><a name="l00063"></a><span class="lineno">   63</span>&#160;</div><div class="line"><a name="l00064"></a><span class="lineno">   64</span>&#160;  <span class="comment">// from tips to base</span></div><div class="line"><a name="l00065"></a><span class="lineno">   65</span>&#160;  <span class="keywordflow">while</span> (i &gt; 5) {</div><div class="line"><a name="l00066"></a><span class="lineno">   66</span>&#160;    tmp = F.dot(_S[i]);</div><div class="line"><a name="l00067"></a><span class="lineno">   67</span>&#160;    LambdaInv += tmp * tmp / _d[i];</div><div class="line"><a name="l00068"></a><span class="lineno">   68</span>&#160;    dstate_out.tail(_nDof - 6) += _qdd_from_subqdd.col(i - 6) * tmp / _d[i];</div><div class="line"><a name="l00069"></a><span class="lineno">   69</span>&#160;</div><div class="line"><a name="l00070"></a><span class="lineno">   70</span>&#160;    <span class="comment">// Apply force propagator (see Pat&#39;s ICRA 2012 paper)</span></div><div class="line"><a name="l00071"></a><span class="lineno">   71</span>&#160;    <span class="comment">// essentially, since the joint is articulated, only a portion of the force</span></div><div class="line"><a name="l00072"></a><span class="lineno">   72</span>&#160;    <span class="comment">// is felt on the predecessor. So, while Xup^T sends a force backwards as if</span></div><div class="line"><a name="l00073"></a><span class="lineno">   73</span>&#160;    <span class="comment">// the joint was locked, ChiUp^T sends the force backward as if the joint</span></div><div class="line"><a name="l00074"></a><span class="lineno">   74</span>&#160;    <span class="comment">// were free</span></div><div class="line"><a name="l00075"></a><span class="lineno">   75</span>&#160;    F = _ChiUp[i].transpose() * F;</div><div class="line"><a name="l00076"></a><span class="lineno">   76</span>&#160;    i = _parents[i];</div><div class="line"><a name="l00077"></a><span class="lineno">   77</span>&#160;  }</div><div class="line"><a name="l00078"></a><span class="lineno">   78</span>&#160;</div><div class="line"><a name="l00079"></a><span class="lineno">   79</span>&#160;  dstate_out.head(6) = _invIA5.solve(F);</div><div class="line"><a name="l00080"></a><span class="lineno">   80</span>&#160;  LambdaInv += F.dot(dstate_out.head(6));</div><div class="line"><a name="l00081"></a><span class="lineno">   81</span>&#160;  dstate_out.tail(_nDof - 6) += _qdd_from_base_accel * dstate_out.head(6);</div><div class="line"><a name="l00082"></a><span class="lineno">   82</span>&#160;</div><div class="line"><a name="l00083"></a><span class="lineno">   83</span>&#160;  <span class="keywordflow">return</span> LambdaInv;</div><div class="line"><a name="l00084"></a><span class="lineno">   84</span>&#160;}</div><div class="line"><a name="l00085"></a><span class="lineno">   85</span>&#160;</div><div class="line"><a name="l00097"></a><span class="lineno">   97</span>&#160;<span class="keyword">template</span> &lt;<span class="keyword">typename</span> T&gt;</div><div class="line"><a name="l00098"></a><span class="lineno"><a class="line" href="../../d6/db1/class_floating_base_model.html#ae7c9130e9835487677c82edd2918f4e0">   98</a></span>&#160;<span class="keywordtype">void</span> <a class="code" href="../../d6/db1/class_floating_base_model.html#ae7c9130e9835487677c82edd2918f4e0">FloatingBaseModel&lt;T&gt;::udpateQddEffects</a>() {</div><div class="line"><a name="l00099"></a><span class="lineno">   99</span>&#160;  <span class="keywordflow">if</span> (_qddEffectsUpToDate) <span class="keywordflow">return</span>;</div><div class="line"><a name="l00100"></a><span class="lineno">  100</span>&#160;  updateForcePropagators();</div><div class="line"><a name="l00101"></a><span class="lineno">  101</span>&#160;  _qdd_from_base_accel.setZero();</div><div class="line"><a name="l00102"></a><span class="lineno">  102</span>&#160;  _qdd_from_subqdd.setZero();</div><div class="line"><a name="l00103"></a><span class="lineno">  103</span>&#160;</div><div class="line"><a name="l00104"></a><span class="lineno">  104</span>&#160;  <span class="comment">// Pass for force props</span></div><div class="line"><a name="l00105"></a><span class="lineno">  105</span>&#160;  <span class="comment">// This loop is semi-equivalent to a cholesky factorization on H</span></div><div class="line"><a name="l00106"></a><span class="lineno">  106</span>&#160;  <span class="comment">// akin to Featherstone&#39;s sparse operational space algo</span></div><div class="line"><a name="l00107"></a><span class="lineno">  107</span>&#160;  <span class="comment">// These computations are for treating the joint rates like a task space</span></div><div class="line"><a name="l00108"></a><span class="lineno">  108</span>&#160;  <span class="comment">// To do so, F computes the dynamic effect of torues onto bodies down the tree</span></div><div class="line"><a name="l00109"></a><span class="lineno">  109</span>&#160;  <span class="comment">//</span></div><div class="line"><a name="l00110"></a><span class="lineno">  110</span>&#160;  <span class="keywordflow">for</span> (<span class="keywordtype">size_t</span> i = 6; i &lt; _nDof; i++) {</div><div class="line"><a name="l00111"></a><span class="lineno">  111</span>&#160;    _qdd_from_subqdd(i - 6, i - 6) = 1;</div><div class="line"><a name="l00112"></a><span class="lineno">  112</span>&#160;    <a class="code" href="../../d9/d8e/cpp_types_8h.html#a3d70f8eb9cf1855ca989f887a5543acc">SVec&lt;T&gt;</a> F = (_ChiUp[i].transpose() - _Xup[i].transpose()) * _S[i];</div><div class="line"><a name="l00113"></a><span class="lineno">  113</span>&#160;    <span class="keywordtype">size_t</span> j = _parents[i];</div><div class="line"><a name="l00114"></a><span class="lineno">  114</span>&#160;    <span class="keywordflow">while</span> (j &gt; 5) {</div><div class="line"><a name="l00115"></a><span class="lineno">  115</span>&#160;      _qdd_from_subqdd(i - 6, j - 6) = _S[j].dot(F);</div><div class="line"><a name="l00116"></a><span class="lineno">  116</span>&#160;      F = _ChiUp[j].transpose() * F;</div><div class="line"><a name="l00117"></a><span class="lineno">  117</span>&#160;      j = _parents[j];</div><div class="line"><a name="l00118"></a><span class="lineno">  118</span>&#160;    }</div><div class="line"><a name="l00119"></a><span class="lineno">  119</span>&#160;    _qdd_from_base_accel.row(i - 6) = F.transpose();</div><div class="line"><a name="l00120"></a><span class="lineno">  120</span>&#160;  }</div><div class="line"><a name="l00121"></a><span class="lineno">  121</span>&#160;  _qddEffectsUpToDate = <span class="keyword">true</span>;</div><div class="line"><a name="l00122"></a><span class="lineno">  122</span>&#160;}</div><div class="line"><a name="l00123"></a><span class="lineno">  123</span>&#160;</div><div class="line"><a name="l00128"></a><span class="lineno">  128</span>&#160;<span class="keyword">template</span> &lt;<span class="keyword">typename</span> T&gt;</div><div class="line"><a name="l00129"></a><span class="lineno"><a class="line" href="../../d6/db1/class_floating_base_model.html#a409343ac03c6aebaffdb288c6c65f892">  129</a></span>&#160;<span class="keywordtype">void</span> <a class="code" href="../../d6/db1/class_floating_base_model.html#a409343ac03c6aebaffdb288c6c65f892">FloatingBaseModel&lt;T&gt;::updateForcePropagators</a>() {</div><div class="line"><a name="l00130"></a><span class="lineno">  130</span>&#160;  <span class="keywordflow">if</span> (_forcePropagatorsUpToDate) <span class="keywordflow">return</span>;</div><div class="line"><a name="l00131"></a><span class="lineno">  131</span>&#160;  updateArticulatedBodies();</div><div class="line"><a name="l00132"></a><span class="lineno">  132</span>&#160;  <span class="keywordflow">for</span> (<span class="keywordtype">size_t</span> i = 6; i &lt; _nDof; i++) {</div><div class="line"><a name="l00133"></a><span class="lineno">  133</span>&#160;    _ChiUp[i] = _Xup[i] - _S[i] * _Utot[i].transpose() / _d[i];</div><div class="line"><a name="l00134"></a><span class="lineno">  134</span>&#160;  }</div><div class="line"><a name="l00135"></a><span class="lineno">  135</span>&#160;  _forcePropagatorsUpToDate = <span class="keyword">true</span>;</div><div class="line"><a name="l00136"></a><span class="lineno">  136</span>&#160;}</div><div class="line"><a name="l00137"></a><span class="lineno">  137</span>&#160;</div><div class="line"><a name="l00141"></a><span class="lineno">  141</span>&#160;<span class="keyword">template</span> &lt;<span class="keyword">typename</span> T&gt;</div><div class="line"><a name="l00142"></a><span class="lineno"><a class="line" href="../../d6/db1/class_floating_base_model.html#aad1c033e1d94d42ee028accaf6a48b1e">  142</a></span>&#160;<span class="keywordtype">void</span> <a class="code" href="../../d6/db1/class_floating_base_model.html#aad1c033e1d94d42ee028accaf6a48b1e">FloatingBaseModel&lt;T&gt;::updateArticulatedBodies</a>() {</div><div class="line"><a name="l00143"></a><span class="lineno">  143</span>&#160;  <span class="keywordflow">if</span> (_articulatedBodiesUpToDate) <span class="keywordflow">return</span>;</div><div class="line"><a name="l00144"></a><span class="lineno">  144</span>&#160;</div><div class="line"><a name="l00145"></a><span class="lineno">  145</span>&#160;  forwardKinematics();</div><div class="line"><a name="l00146"></a><span class="lineno">  146</span>&#160;</div><div class="line"><a name="l00147"></a><span class="lineno">  147</span>&#160;  _IA[5] = _Ibody[5].getMatrix();</div><div class="line"><a name="l00148"></a><span class="lineno">  148</span>&#160;</div><div class="line"><a name="l00149"></a><span class="lineno">  149</span>&#160;  <span class="comment">// loop 1, down the tree</span></div><div class="line"><a name="l00150"></a><span class="lineno">  150</span>&#160;  <span class="keywordflow">for</span> (<span class="keywordtype">size_t</span> i = 6; i &lt; _nDof; i++) {</div><div class="line"><a name="l00151"></a><span class="lineno">  151</span>&#160;    _IA[i] = _Ibody[i].getMatrix();  <span class="comment">// initialize</span></div><div class="line"><a name="l00152"></a><span class="lineno">  152</span>&#160;    <a class="code" href="../../d9/d8e/cpp_types_8h.html#ae8ab6bd4079a5317297be5eeb331b1ca">Mat6&lt;T&gt;</a> XJrot = <a class="code" href="../../de/d1a/namespacespatial.html#a6440807f3b1854b1708f49048331811a">jointXform</a>(_jointTypes[i], _jointAxes[i],</div><div class="line"><a name="l00153"></a><span class="lineno">  153</span>&#160;                               _state.q[i - 6] * _gearRatios[i]);</div><div class="line"><a name="l00154"></a><span class="lineno">  154</span>&#160;    _Xuprot[i] = XJrot * _Xrot[i];</div><div class="line"><a name="l00155"></a><span class="lineno">  155</span>&#160;    _Srot[i] = _S[i] * _gearRatios[i];</div><div class="line"><a name="l00156"></a><span class="lineno">  156</span>&#160;  }</div><div class="line"><a name="l00157"></a><span class="lineno">  157</span>&#160;</div><div class="line"><a name="l00158"></a><span class="lineno">  158</span>&#160;  <span class="comment">// Pat&#39;s magic principle of least constraint (Guass too!)</span></div><div class="line"><a name="l00159"></a><span class="lineno">  159</span>&#160;  <span class="keywordflow">for</span> (<span class="keywordtype">size_t</span> i = _nDof - 1; i &gt;= 6; i--) {</div><div class="line"><a name="l00160"></a><span class="lineno">  160</span>&#160;    _U[i] = _IA[i] * _S[i];</div><div class="line"><a name="l00161"></a><span class="lineno">  161</span>&#160;    _Urot[i] = _Irot[i].getMatrix() * _Srot[i];</div><div class="line"><a name="l00162"></a><span class="lineno">  162</span>&#160;    _Utot[i] = _Xup[i].transpose() * _U[i] + _Xuprot[i].transpose() * _Urot[i];</div><div class="line"><a name="l00163"></a><span class="lineno">  163</span>&#160;</div><div class="line"><a name="l00164"></a><span class="lineno">  164</span>&#160;    _d[i] = _Srot[i].transpose() * _Urot[i];</div><div class="line"><a name="l00165"></a><span class="lineno">  165</span>&#160;    _d[i] += _S[i].transpose() * _U[i];</div><div class="line"><a name="l00166"></a><span class="lineno">  166</span>&#160;</div><div class="line"><a name="l00167"></a><span class="lineno">  167</span>&#160;    <span class="comment">// articulated inertia recursion</span></div><div class="line"><a name="l00168"></a><span class="lineno">  168</span>&#160;    <a class="code" href="../../d9/d8e/cpp_types_8h.html#ae8ab6bd4079a5317297be5eeb331b1ca">Mat6&lt;T&gt;</a> Ia = _Xup[i].transpose() * _IA[i] * _Xup[i] +</div><div class="line"><a name="l00169"></a><span class="lineno">  169</span>&#160;                 _Xuprot[i].transpose() * _Irot[i].getMatrix() * _Xuprot[i] -</div><div class="line"><a name="l00170"></a><span class="lineno">  170</span>&#160;                 _Utot[i] * _Utot[i].transpose() / _d[i];</div><div class="line"><a name="l00171"></a><span class="lineno">  171</span>&#160;    _IA[_parents[i]] += Ia;</div><div class="line"><a name="l00172"></a><span class="lineno">  172</span>&#160;  }</div><div class="line"><a name="l00173"></a><span class="lineno">  173</span>&#160;</div><div class="line"><a name="l00174"></a><span class="lineno">  174</span>&#160;  _invIA5.compute(_IA[5]);</div><div class="line"><a name="l00175"></a><span class="lineno">  175</span>&#160;  _articulatedBodiesUpToDate = <span class="keyword">true</span>;</div><div class="line"><a name="l00176"></a><span class="lineno">  176</span>&#160;}</div><div class="line"><a name="l00177"></a><span class="lineno">  177</span>&#160;</div><div class="line"><a name="l00178"></a><span class="lineno">  178</span>&#160;<span class="comment">// parents, gr, jtype, Xtree, I, Xrot, Irot,</span></div><div class="line"><a name="l00179"></a><span class="lineno">  179</span>&#160;</div><div class="line"><a name="l00184"></a><span class="lineno">  184</span>&#160;<span class="keyword">template</span> &lt;<span class="keyword">typename</span> T&gt;</div><div class="line"><a name="l00185"></a><span class="lineno"><a class="line" href="../../d6/db1/class_floating_base_model.html#aee861a77ddcefb97d62f0f535ee97508">  185</a></span>&#160;<span class="keywordtype">void</span> <a class="code" href="../../d6/db1/class_floating_base_model.html#aee861a77ddcefb97d62f0f535ee97508">FloatingBaseModel&lt;T&gt;::addDynamicsVars</a>(<span class="keywordtype">int</span> count) {</div><div class="line"><a name="l00186"></a><span class="lineno">  186</span>&#160;  <span class="keywordflow">if</span> (count != 1 &amp;&amp; count != 6) {</div><div class="line"><a name="l00187"></a><span class="lineno">  187</span>&#160;    <span class="keywordflow">throw</span> std::runtime_error(</div><div class="line"><a name="l00188"></a><span class="lineno">  188</span>&#160;        <span class="stringliteral">&quot;addDynamicsVars must be called with count=1 (joint) or count=6 &quot;</span></div><div class="line"><a name="l00189"></a><span class="lineno">  189</span>&#160;        <span class="stringliteral">&quot;(base).\n&quot;</span>);</div><div class="line"><a name="l00190"></a><span class="lineno">  190</span>&#160;  }</div><div class="line"><a name="l00191"></a><span class="lineno">  191</span>&#160;</div><div class="line"><a name="l00192"></a><span class="lineno">  192</span>&#160;  <a class="code" href="../../d9/d8e/cpp_types_8h.html#ae8ab6bd4079a5317297be5eeb331b1ca">Mat6&lt;T&gt;</a> eye6 = <a class="code" href="../../d9/d8e/cpp_types_8h.html#ae8ab6bd4079a5317297be5eeb331b1ca">Mat6&lt;T&gt;::Identity</a>();</div><div class="line"><a name="l00193"></a><span class="lineno">  193</span>&#160;  <a class="code" href="../../d9/d8e/cpp_types_8h.html#a3d70f8eb9cf1855ca989f887a5543acc">SVec&lt;T&gt;</a> zero6 = <a class="code" href="../../d9/d8e/cpp_types_8h.html#a3d70f8eb9cf1855ca989f887a5543acc">SVec&lt;T&gt;::Zero</a>();</div><div class="line"><a name="l00194"></a><span class="lineno">  194</span>&#160;  <a class="code" href="../../d9/d8e/cpp_types_8h.html#ae8ab6bd4079a5317297be5eeb331b1ca">Mat6&lt;T&gt;</a> zero66 = <a class="code" href="../../d9/d8e/cpp_types_8h.html#ae8ab6bd4079a5317297be5eeb331b1ca">Mat6&lt;T&gt;::Zero</a>();</div><div class="line"><a name="l00195"></a><span class="lineno">  195</span>&#160;</div><div class="line"><a name="l00196"></a><span class="lineno">  196</span>&#160;  <a class="code" href="../../d6/d81/class_spatial_inertia.html">SpatialInertia&lt;T&gt;</a> zeroInertia(zero66);</div><div class="line"><a name="l00197"></a><span class="lineno">  197</span>&#160;  <span class="keywordflow">for</span> (<span class="keywordtype">int</span> i = 0; i &lt; count; i++) {</div><div class="line"><a name="l00198"></a><span class="lineno">  198</span>&#160;    _v.push_back(zero6);</div><div class="line"><a name="l00199"></a><span class="lineno">  199</span>&#160;    _vrot.push_back(zero6);</div><div class="line"><a name="l00200"></a><span class="lineno">  200</span>&#160;    _a.push_back(zero6);</div><div class="line"><a name="l00201"></a><span class="lineno">  201</span>&#160;    _arot.push_back(zero6);</div><div class="line"><a name="l00202"></a><span class="lineno">  202</span>&#160;    _avp.push_back(zero6);</div><div class="line"><a name="l00203"></a><span class="lineno">  203</span>&#160;    _avprot.push_back(zero6);</div><div class="line"><a name="l00204"></a><span class="lineno">  204</span>&#160;    _c.push_back(zero6);</div><div class="line"><a name="l00205"></a><span class="lineno">  205</span>&#160;    _crot.push_back(zero6);</div><div class="line"><a name="l00206"></a><span class="lineno">  206</span>&#160;    _S.push_back(zero6);</div><div class="line"><a name="l00207"></a><span class="lineno">  207</span>&#160;    _Srot.push_back(zero6);</div><div class="line"><a name="l00208"></a><span class="lineno">  208</span>&#160;    _f.push_back(zero6);</div><div class="line"><a name="l00209"></a><span class="lineno">  209</span>&#160;    _frot.push_back(zero6);</div><div class="line"><a name="l00210"></a><span class="lineno">  210</span>&#160;    _fvp.push_back(zero6);</div><div class="line"><a name="l00211"></a><span class="lineno">  211</span>&#160;    _fvprot.push_back(zero6);</div><div class="line"><a name="l00212"></a><span class="lineno">  212</span>&#160;    _ag.push_back(zero6);</div><div class="line"><a name="l00213"></a><span class="lineno">  213</span>&#160;    _agrot.push_back(zero6);</div><div class="line"><a name="l00214"></a><span class="lineno">  214</span>&#160;    _IC.push_back(zeroInertia);</div><div class="line"><a name="l00215"></a><span class="lineno">  215</span>&#160;    _Xup.push_back(eye6);</div><div class="line"><a name="l00216"></a><span class="lineno">  216</span>&#160;    _Xuprot.push_back(eye6);</div><div class="line"><a name="l00217"></a><span class="lineno">  217</span>&#160;    _Xa.push_back(eye6);</div><div class="line"><a name="l00218"></a><span class="lineno">  218</span>&#160;</div><div class="line"><a name="l00219"></a><span class="lineno">  219</span>&#160;    _ChiUp.push_back(eye6);</div><div class="line"><a name="l00220"></a><span class="lineno">  220</span>&#160;    _d.push_back(0.);</div><div class="line"><a name="l00221"></a><span class="lineno">  221</span>&#160;    _u.push_back(0.);</div><div class="line"><a name="l00222"></a><span class="lineno">  222</span>&#160;    _IA.push_back(eye6);</div><div class="line"><a name="l00223"></a><span class="lineno">  223</span>&#160;</div><div class="line"><a name="l00224"></a><span class="lineno">  224</span>&#160;    _U.push_back(zero6);</div><div class="line"><a name="l00225"></a><span class="lineno">  225</span>&#160;    _Urot.push_back(zero6);</div><div class="line"><a name="l00226"></a><span class="lineno">  226</span>&#160;    _Utot.push_back(zero6);</div><div class="line"><a name="l00227"></a><span class="lineno">  227</span>&#160;    _pA.push_back(zero6);</div><div class="line"><a name="l00228"></a><span class="lineno">  228</span>&#160;    _pArot.push_back(zero6);</div><div class="line"><a name="l00229"></a><span class="lineno">  229</span>&#160;    _externalForces.push_back(zero6);</div><div class="line"><a name="l00230"></a><span class="lineno">  230</span>&#160;  }</div><div class="line"><a name="l00231"></a><span class="lineno">  231</span>&#160;</div><div class="line"><a name="l00232"></a><span class="lineno">  232</span>&#160;  _J.push_back(<a class="code" href="../../d9/d8e/cpp_types_8h.html#a0231caceb234a1eace8c03dda00431dd">D6Mat&lt;T&gt;::Zero</a>(6, _nDof));</div><div class="line"><a name="l00233"></a><span class="lineno">  233</span>&#160;  _Jdqd.push_back(<a class="code" href="../../d9/d8e/cpp_types_8h.html#a3d70f8eb9cf1855ca989f887a5543acc">SVec&lt;T&gt;::Zero</a>());</div><div class="line"><a name="l00234"></a><span class="lineno">  234</span>&#160;</div><div class="line"><a name="l00235"></a><span class="lineno">  235</span>&#160;  resizeSystemMatricies();</div><div class="line"><a name="l00236"></a><span class="lineno">  236</span>&#160;}</div><div class="line"><a name="l00237"></a><span class="lineno">  237</span>&#160;</div><div class="line"><a name="l00241"></a><span class="lineno">  241</span>&#160;<span class="keyword">template</span> &lt;<span class="keyword">typename</span> T&gt;</div><div class="line"><a name="l00242"></a><span class="lineno"><a class="line" href="../../d6/db1/class_floating_base_model.html#a8caeb9858d5b992b6e9f9d525be81376">  242</a></span>&#160;<span class="keywordtype">void</span> <a class="code" href="../../d6/db1/class_floating_base_model.html#a8caeb9858d5b992b6e9f9d525be81376">FloatingBaseModel&lt;T&gt;::resizeSystemMatricies</a>() {</div><div class="line"><a name="l00243"></a><span class="lineno">  243</span>&#160;  _H.setZero(_nDof, _nDof);</div><div class="line"><a name="l00244"></a><span class="lineno">  244</span>&#160;  _C.setZero(_nDof, _nDof);</div><div class="line"><a name="l00245"></a><span class="lineno">  245</span>&#160;  _Cqd.setZero(_nDof);</div><div class="line"><a name="l00246"></a><span class="lineno">  246</span>&#160;  _G.setZero(_nDof);</div><div class="line"><a name="l00247"></a><span class="lineno">  247</span>&#160;  <span class="keywordflow">for</span> (<span class="keywordtype">size_t</span> i = 0; i &lt; _J.size(); i++) {</div><div class="line"><a name="l00248"></a><span class="lineno">  248</span>&#160;    _J[i].setZero(6, _nDof);</div><div class="line"><a name="l00249"></a><span class="lineno">  249</span>&#160;    _Jdqd[i].setZero();</div><div class="line"><a name="l00250"></a><span class="lineno">  250</span>&#160;  }</div><div class="line"><a name="l00251"></a><span class="lineno">  251</span>&#160;</div><div class="line"><a name="l00252"></a><span class="lineno">  252</span>&#160;  <span class="keywordflow">for</span> (<span class="keywordtype">size_t</span> i = 0; i &lt; _Jc.size(); i++) {</div><div class="line"><a name="l00253"></a><span class="lineno">  253</span>&#160;    _Jc[i].setZero(3, _nDof);</div><div class="line"><a name="l00254"></a><span class="lineno">  254</span>&#160;    _Jcdqd[i].setZero();</div><div class="line"><a name="l00255"></a><span class="lineno">  255</span>&#160;  }</div><div class="line"><a name="l00256"></a><span class="lineno">  256</span>&#160;  _qdd_from_subqdd.resize(_nDof - 6, _nDof - 6);</div><div class="line"><a name="l00257"></a><span class="lineno">  257</span>&#160;  _qdd_from_base_accel.resize(_nDof - 6, 6);</div><div class="line"><a name="l00258"></a><span class="lineno">  258</span>&#160;  _state.q = <a class="code" href="../../d9/d8e/cpp_types_8h.html#ae2e5a0a178a2c528d8aa939f85db7a3e">DVec&lt;T&gt;::Zero</a>(_nDof - 6);</div><div class="line"><a name="l00259"></a><span class="lineno">  259</span>&#160;  _state.qd = <a class="code" href="../../d9/d8e/cpp_types_8h.html#ae2e5a0a178a2c528d8aa939f85db7a3e">DVec&lt;T&gt;::Zero</a>(_nDof - 6);</div><div class="line"><a name="l00260"></a><span class="lineno">  260</span>&#160;}</div><div class="line"><a name="l00261"></a><span class="lineno">  261</span>&#160;</div><div class="line"><a name="l00266"></a><span class="lineno">  266</span>&#160;<span class="keyword">template</span> &lt;<span class="keyword">typename</span> T&gt;</div><div class="line"><a name="l00267"></a><span class="lineno"><a class="line" href="../../d6/db1/class_floating_base_model.html#ab3dfc6605561bee26b4f7a893e44c78d">  267</a></span>&#160;<span class="keywordtype">void</span> <a class="code" href="../../d6/db1/class_floating_base_model.html#ab3dfc6605561bee26b4f7a893e44c78d">FloatingBaseModel&lt;T&gt;::addBase</a>(<span class="keyword">const</span> <a class="code" href="../../d6/d81/class_spatial_inertia.html">SpatialInertia&lt;T&gt;</a> &amp;inertia) {</div><div class="line"><a name="l00268"></a><span class="lineno">  268</span>&#160;  <span class="keywordflow">if</span> (_nDof) {</div><div class="line"><a name="l00269"></a><span class="lineno">  269</span>&#160;    <span class="keywordflow">throw</span> std::runtime_error(<span class="stringliteral">&quot;Cannot add base multiple times!\n&quot;</span>);</div><div class="line"><a name="l00270"></a><span class="lineno">  270</span>&#160;  }</div><div class="line"><a name="l00271"></a><span class="lineno">  271</span>&#160;</div><div class="line"><a name="l00272"></a><span class="lineno">  272</span>&#160;  <a class="code" href="../../d9/d8e/cpp_types_8h.html#ae8ab6bd4079a5317297be5eeb331b1ca">Mat6&lt;T&gt;</a> eye6 = <a class="code" href="../../d9/d8e/cpp_types_8h.html#ae8ab6bd4079a5317297be5eeb331b1ca">Mat6&lt;T&gt;::Identity</a>();</div><div class="line"><a name="l00273"></a><span class="lineno">  273</span>&#160;  <a class="code" href="../../d9/d8e/cpp_types_8h.html#ae8ab6bd4079a5317297be5eeb331b1ca">Mat6&lt;T&gt;</a> zero6 = <a class="code" href="../../d9/d8e/cpp_types_8h.html#ae8ab6bd4079a5317297be5eeb331b1ca">Mat6&lt;T&gt;::Zero</a>();</div><div class="line"><a name="l00274"></a><span class="lineno">  274</span>&#160;  <a class="code" href="../../d6/d81/class_spatial_inertia.html">SpatialInertia&lt;T&gt;</a> zeroInertia(zero6);</div><div class="line"><a name="l00275"></a><span class="lineno">  275</span>&#160;  <span class="comment">// the floating base has 6 DOFs</span></div><div class="line"><a name="l00276"></a><span class="lineno">  276</span>&#160;</div><div class="line"><a name="l00277"></a><span class="lineno">  277</span>&#160;  _nDof = 6;</div><div class="line"><a name="l00278"></a><span class="lineno">  278</span>&#160;  <span class="keywordflow">for</span> (<span class="keywordtype">size_t</span> i = 0; i &lt; 6; i++) {</div><div class="line"><a name="l00279"></a><span class="lineno">  279</span>&#160;    _parents.push_back(0);</div><div class="line"><a name="l00280"></a><span class="lineno">  280</span>&#160;    _gearRatios.push_back(0);</div><div class="line"><a name="l00281"></a><span class="lineno">  281</span>&#160;    _jointTypes.push_back(JointType::Nothing);  <span class="comment">// doesn&#39;t actually matter</span></div><div class="line"><a name="l00282"></a><span class="lineno">  282</span>&#160;    _jointAxes.push_back(<a class="code" href="../../da/d9b/namespaceori.html#a02c20a1b1d35988c657aa1e219d609d5a02129bb861061d1a052c592e2dc6b383">CoordinateAxis::X</a>);    <span class="comment">// doesn&#39;t actually matter</span></div><div class="line"><a name="l00283"></a><span class="lineno">  283</span>&#160;    _Xtree.push_back(eye6);</div><div class="line"><a name="l00284"></a><span class="lineno">  284</span>&#160;    _Ibody.push_back(zeroInertia);</div><div class="line"><a name="l00285"></a><span class="lineno">  285</span>&#160;    _Xrot.push_back(eye6);</div><div class="line"><a name="l00286"></a><span class="lineno">  286</span>&#160;    _Irot.push_back(zeroInertia);</div><div class="line"><a name="l00287"></a><span class="lineno">  287</span>&#160;    _bodyNames.push_back(<span class="stringliteral">&quot;N/A&quot;</span>);</div><div class="line"><a name="l00288"></a><span class="lineno">  288</span>&#160;  }</div><div class="line"><a name="l00289"></a><span class="lineno">  289</span>&#160;</div><div class="line"><a name="l00290"></a><span class="lineno">  290</span>&#160;  _jointTypes[5] = JointType::FloatingBase;</div><div class="line"><a name="l00291"></a><span class="lineno">  291</span>&#160;  _Ibody[5] = inertia;</div><div class="line"><a name="l00292"></a><span class="lineno">  292</span>&#160;  _gearRatios[5] = 1;</div><div class="line"><a name="l00293"></a><span class="lineno">  293</span>&#160;  _bodyNames[5] = <span class="stringliteral">&quot;Floating Base&quot;</span>;</div><div class="line"><a name="l00294"></a><span class="lineno">  294</span>&#160;</div><div class="line"><a name="l00295"></a><span class="lineno">  295</span>&#160;  addDynamicsVars(6);</div><div class="line"><a name="l00296"></a><span class="lineno">  296</span>&#160;}</div><div class="line"><a name="l00297"></a><span class="lineno">  297</span>&#160;</div><div class="line"><a name="l00304"></a><span class="lineno">  304</span>&#160;<span class="keyword">template</span> &lt;<span class="keyword">typename</span> T&gt;</div><div class="line"><a name="l00305"></a><span class="lineno"><a class="line" href="../../d6/db1/class_floating_base_model.html#a845cab917d2763b5c8ad35f300411acc">  305</a></span>&#160;<span class="keywordtype">void</span> <a class="code" href="../../d6/db1/class_floating_base_model.html#ab3dfc6605561bee26b4f7a893e44c78d">FloatingBaseModel&lt;T&gt;::addBase</a>(T mass, <span class="keyword">const</span> <a class="code" href="../../d9/d8e/cpp_types_8h.html#a6f07728c866b15ff0676a9b2ebfd2ba6">Vec3&lt;T&gt;</a> &amp;com,</div><div class="line"><a name="l00306"></a><span class="lineno">  306</span>&#160;                                   <span class="keyword">const</span> <a class="code" href="../../d9/d8e/cpp_types_8h.html#aa8cb424e643d476ea485238dad7c3e18">Mat3&lt;T&gt;</a> &amp;I) {</div><div class="line"><a name="l00307"></a><span class="lineno">  307</span>&#160;  <a class="code" href="../../d6/d81/class_spatial_inertia.html">SpatialInertia&lt;T&gt;</a> IS(mass, com, I);</div><div class="line"><a name="l00308"></a><span class="lineno">  308</span>&#160;  addBase(IS);</div><div class="line"><a name="l00309"></a><span class="lineno">  309</span>&#160;}</div><div class="line"><a name="l00310"></a><span class="lineno">  310</span>&#160;</div><div class="line"><a name="l00318"></a><span class="lineno">  318</span>&#160;<span class="keyword">template</span> &lt;<span class="keyword">typename</span> T&gt;</div><div class="line"><a name="l00319"></a><span class="lineno"><a class="line" href="../../d6/db1/class_floating_base_model.html#a3f2a319bbd26e5e7f91c7d3b980e28c4">  319</a></span>&#160;<span class="keywordtype">int</span> <a class="code" href="../../d6/db1/class_floating_base_model.html#a3f2a319bbd26e5e7f91c7d3b980e28c4">FloatingBaseModel&lt;T&gt;::addGroundContactPoint</a>(<span class="keywordtype">int</span> bodyID,</div><div class="line"><a name="l00320"></a><span class="lineno">  320</span>&#160;                                                <span class="keyword">const</span> <a class="code" href="../../d9/d8e/cpp_types_8h.html#a6f07728c866b15ff0676a9b2ebfd2ba6">Vec3&lt;T&gt;</a> &amp;location,</div><div class="line"><a name="l00321"></a><span class="lineno">  321</span>&#160;                                                <span class="keywordtype">bool</span> isFoot) {</div><div class="line"><a name="l00322"></a><span class="lineno">  322</span>&#160;  <span class="keywordflow">if</span> ((<span class="keywordtype">size_t</span>)bodyID &gt;= _nDof) {</div><div class="line"><a name="l00323"></a><span class="lineno">  323</span>&#160;    <span class="keywordflow">throw</span> std::runtime_error(</div><div class="line"><a name="l00324"></a><span class="lineno">  324</span>&#160;        <span class="stringliteral">&quot;addGroundContactPoint got invalid bodyID: &quot;</span> + std::to_string(bodyID) +</div><div class="line"><a name="l00325"></a><span class="lineno">  325</span>&#160;        <span class="stringliteral">&quot; nDofs: &quot;</span> + std::to_string(_nDof) + <span class="stringliteral">&quot;\n&quot;</span>);</div><div class="line"><a name="l00326"></a><span class="lineno">  326</span>&#160;  }</div><div class="line"><a name="l00327"></a><span class="lineno">  327</span>&#160;</div><div class="line"><a name="l00328"></a><span class="lineno">  328</span>&#160;  <span class="comment">// std::cout &lt;&lt; &quot;pt-add: &quot; &lt;&lt; location.transpose() &lt;&lt; &quot;\n&quot;;</span></div><div class="line"><a name="l00329"></a><span class="lineno">  329</span>&#160;  _gcParent.push_back(bodyID);</div><div class="line"><a name="l00330"></a><span class="lineno">  330</span>&#160;  _gcLocation.push_back(location);</div><div class="line"><a name="l00331"></a><span class="lineno">  331</span>&#160;</div><div class="line"><a name="l00332"></a><span class="lineno">  332</span>&#160;  <a class="code" href="../../d9/d8e/cpp_types_8h.html#a6f07728c866b15ff0676a9b2ebfd2ba6">Vec3&lt;T&gt;</a> zero3 = <a class="code" href="../../d9/d8e/cpp_types_8h.html#a6f07728c866b15ff0676a9b2ebfd2ba6">Vec3&lt;T&gt;::Zero</a>();</div><div class="line"><a name="l00333"></a><span class="lineno">  333</span>&#160;</div><div class="line"><a name="l00334"></a><span class="lineno">  334</span>&#160;  _pGC.push_back(zero3);</div><div class="line"><a name="l00335"></a><span class="lineno">  335</span>&#160;  _vGC.push_back(zero3);</div><div class="line"><a name="l00336"></a><span class="lineno">  336</span>&#160;</div><div class="line"><a name="l00337"></a><span class="lineno">  337</span>&#160;  <a class="code" href="../../d9/d8e/cpp_types_8h.html#a190e396ab8cb0eb974ad2a5195edcb24">D3Mat&lt;T&gt;</a> J(3, _nDof);</div><div class="line"><a name="l00338"></a><span class="lineno">  338</span>&#160;  J.setZero();</div><div class="line"><a name="l00339"></a><span class="lineno">  339</span>&#160;</div><div class="line"><a name="l00340"></a><span class="lineno">  340</span>&#160;  _Jc.push_back(J);</div><div class="line"><a name="l00341"></a><span class="lineno">  341</span>&#160;  _Jcdqd.push_back(zero3);</div><div class="line"><a name="l00342"></a><span class="lineno">  342</span>&#160;  <span class="comment">//_compute_contact_info.push_back(false);</span></div><div class="line"><a name="l00343"></a><span class="lineno">  343</span>&#160;  _compute_contact_info.push_back(<span class="keyword">true</span>);</div><div class="line"><a name="l00344"></a><span class="lineno">  344</span>&#160;</div><div class="line"><a name="l00345"></a><span class="lineno">  345</span>&#160;  <span class="comment">// add foot to foot list</span></div><div class="line"><a name="l00346"></a><span class="lineno">  346</span>&#160;  <span class="keywordflow">if</span> (isFoot) {</div><div class="line"><a name="l00347"></a><span class="lineno">  347</span>&#160;    _footIndicesGC.push_back(_nGroundContact);</div><div class="line"><a name="l00348"></a><span class="lineno">  348</span>&#160;    _compute_contact_info[_nGroundContact] = <span class="keyword">true</span>;</div><div class="line"><a name="l00349"></a><span class="lineno">  349</span>&#160;  }</div><div class="line"><a name="l00350"></a><span class="lineno">  350</span>&#160;</div><div class="line"><a name="l00351"></a><span class="lineno">  351</span>&#160;  resizeSystemMatricies();</div><div class="line"><a name="l00352"></a><span class="lineno">  352</span>&#160;  <span class="keywordflow">return</span> _nGroundContact++;</div><div class="line"><a name="l00353"></a><span class="lineno">  353</span>&#160;}</div><div class="line"><a name="l00354"></a><span class="lineno">  354</span>&#160;</div><div class="line"><a name="l00359"></a><span class="lineno">  359</span>&#160;<span class="keyword">template</span> &lt;<span class="keyword">typename</span> T&gt;</div><div class="line"><a name="l00360"></a><span class="lineno"><a class="line" href="../../d6/db1/class_floating_base_model.html#aff2cddc3077b282fe3504ecd28bc08cb">  360</a></span>&#160;<span class="keywordtype">void</span> <a class="code" href="../../d6/db1/class_floating_base_model.html#aff2cddc3077b282fe3504ecd28bc08cb">FloatingBaseModel&lt;T&gt;::addGroundContactBoxPoints</a>(<span class="keywordtype">int</span> bodyId,</div><div class="line"><a name="l00361"></a><span class="lineno">  361</span>&#160;                                                     <span class="keyword">const</span> <a class="code" href="../../d9/d8e/cpp_types_8h.html#a6f07728c866b15ff0676a9b2ebfd2ba6">Vec3&lt;T&gt;</a> &amp;dims) {</div><div class="line"><a name="l00362"></a><span class="lineno">  362</span>&#160;  <span class="comment">// addGroundContactPoint(bodyId, Vec3&lt;T&gt;( dims(0),  dims(1),  dims(2))/2);</span></div><div class="line"><a name="l00363"></a><span class="lineno">  363</span>&#160;  <span class="comment">// addGroundContactPoint(bodyId, Vec3&lt;T&gt;(-dims(0),  dims(1),  dims(2))/2);</span></div><div class="line"><a name="l00364"></a><span class="lineno">  364</span>&#160;  <span class="comment">// addGroundContactPoint(bodyId, Vec3&lt;T&gt;( dims(0), -dims(1),  dims(2))/2);</span></div><div class="line"><a name="l00365"></a><span class="lineno">  365</span>&#160;  <span class="comment">// addGroundContactPoint(bodyId, Vec3&lt;T&gt;(-dims(0), -dims(1),  dims(2))/2);</span></div><div class="line"><a name="l00366"></a><span class="lineno">  366</span>&#160;</div><div class="line"><a name="l00367"></a><span class="lineno">  367</span>&#160;  addGroundContactPoint(bodyId, <a class="code" href="../../d9/d8e/cpp_types_8h.html#a6f07728c866b15ff0676a9b2ebfd2ba6">Vec3&lt;T&gt;</a>(dims(0), dims(1), 0.) / 2);</div><div class="line"><a name="l00368"></a><span class="lineno">  368</span>&#160;  addGroundContactPoint(bodyId, <a class="code" href="../../d9/d8e/cpp_types_8h.html#a6f07728c866b15ff0676a9b2ebfd2ba6">Vec3&lt;T&gt;</a>(-dims(0), dims(1), 0.) / 2);</div><div class="line"><a name="l00369"></a><span class="lineno">  369</span>&#160;  addGroundContactPoint(bodyId, <a class="code" href="../../d9/d8e/cpp_types_8h.html#a6f07728c866b15ff0676a9b2ebfd2ba6">Vec3&lt;T&gt;</a>(dims(0), -dims(1), 0.) / 2);</div><div class="line"><a name="l00370"></a><span class="lineno">  370</span>&#160;  addGroundContactPoint(bodyId, <a class="code" href="../../d9/d8e/cpp_types_8h.html#a6f07728c866b15ff0676a9b2ebfd2ba6">Vec3&lt;T&gt;</a>(-dims(0), -dims(1), 0.) / 2);</div><div class="line"><a name="l00371"></a><span class="lineno">  371</span>&#160;</div><div class="line"><a name="l00372"></a><span class="lineno">  372</span>&#160;  addGroundContactPoint(bodyId, <a class="code" href="../../d9/d8e/cpp_types_8h.html#a6f07728c866b15ff0676a9b2ebfd2ba6">Vec3&lt;T&gt;</a>(dims(0), dims(1), -dims(2)) / 2);</div><div class="line"><a name="l00373"></a><span class="lineno">  373</span>&#160;  addGroundContactPoint(bodyId, <a class="code" href="../../d9/d8e/cpp_types_8h.html#a6f07728c866b15ff0676a9b2ebfd2ba6">Vec3&lt;T&gt;</a>(-dims(0), dims(1), -dims(2)) / 2);</div><div class="line"><a name="l00374"></a><span class="lineno">  374</span>&#160;  addGroundContactPoint(bodyId, <a class="code" href="../../d9/d8e/cpp_types_8h.html#a6f07728c866b15ff0676a9b2ebfd2ba6">Vec3&lt;T&gt;</a>(dims(0), -dims(1), -dims(2)) / 2);</div><div class="line"><a name="l00375"></a><span class="lineno">  375</span>&#160;  addGroundContactPoint(bodyId, <a class="code" href="../../d9/d8e/cpp_types_8h.html#a6f07728c866b15ff0676a9b2ebfd2ba6">Vec3&lt;T&gt;</a>(-dims(0), -dims(1), -dims(2)) / 2);</div><div class="line"><a name="l00376"></a><span class="lineno">  376</span>&#160;}</div><div class="line"><a name="l00377"></a><span class="lineno">  377</span>&#160;</div><div class="line"><a name="l00391"></a><span class="lineno">  391</span>&#160;<span class="keyword">template</span> &lt;<span class="keyword">typename</span> T&gt;</div><div class="line"><a name="l00392"></a><span class="lineno"><a class="line" href="../../d6/db1/class_floating_base_model.html#adcff6806f39aedf95a3723fc10c6144e">  392</a></span>&#160;<span class="keywordtype">int</span> <a class="code" href="../../d6/db1/class_floating_base_model.html#adcff6806f39aedf95a3723fc10c6144e">FloatingBaseModel&lt;T&gt;::addBody</a>(<span class="keyword">const</span> <a class="code" href="../../d6/d81/class_spatial_inertia.html">SpatialInertia&lt;T&gt;</a> &amp;inertia,</div><div class="line"><a name="l00393"></a><span class="lineno">  393</span>&#160;                                  <span class="keyword">const</span> <a class="code" href="../../d6/d81/class_spatial_inertia.html">SpatialInertia&lt;T&gt;</a> &amp;rotorInertia,</div><div class="line"><a name="l00394"></a><span class="lineno">  394</span>&#160;                                  T gearRatio, <span class="keywordtype">int</span> parent, <a class="code" href="../../de/d1a/namespacespatial.html#acc851583f244f19d9ea07490e9328b51">JointType</a> jointType,</div><div class="line"><a name="l00395"></a><span class="lineno">  395</span>&#160;                                  <a class="code" href="../../da/d9b/namespaceori.html#a02c20a1b1d35988c657aa1e219d609d5">CoordinateAxis</a> jointAxis,</div><div class="line"><a name="l00396"></a><span class="lineno">  396</span>&#160;                                  <span class="keyword">const</span> <a class="code" href="../../d9/d8e/cpp_types_8h.html#ae8ab6bd4079a5317297be5eeb331b1ca">Mat6&lt;T&gt;</a> &amp;Xtree, <span class="keyword">const</span> <a class="code" href="../../d9/d8e/cpp_types_8h.html#ae8ab6bd4079a5317297be5eeb331b1ca">Mat6&lt;T&gt;</a> &amp;Xrot) {</div><div class="line"><a name="l00397"></a><span class="lineno">  397</span>&#160;  <span class="keywordflow">if</span> ((<span class="keywordtype">size_t</span>)parent &gt;= _nDof) {</div><div class="line"><a name="l00398"></a><span class="lineno">  398</span>&#160;    <span class="keywordflow">throw</span> std::runtime_error(</div><div class="line"><a name="l00399"></a><span class="lineno">  399</span>&#160;        <span class="stringliteral">&quot;addBody got invalid parent: &quot;</span> + std::to_string(parent) +</div><div class="line"><a name="l00400"></a><span class="lineno">  400</span>&#160;        <span class="stringliteral">&quot; nDofs: &quot;</span> + std::to_string(_nDof) + <span class="stringliteral">&quot;\n&quot;</span>);</div><div class="line"><a name="l00401"></a><span class="lineno">  401</span>&#160;  }</div><div class="line"><a name="l00402"></a><span class="lineno">  402</span>&#160;</div><div class="line"><a name="l00403"></a><span class="lineno">  403</span>&#160;  _parents.push_back(parent);</div><div class="line"><a name="l00404"></a><span class="lineno">  404</span>&#160;  _gearRatios.push_back(gearRatio);</div><div class="line"><a name="l00405"></a><span class="lineno">  405</span>&#160;  _jointTypes.push_back(jointType);</div><div class="line"><a name="l00406"></a><span class="lineno">  406</span>&#160;  _jointAxes.push_back(jointAxis);</div><div class="line"><a name="l00407"></a><span class="lineno">  407</span>&#160;  _Xtree.push_back(Xtree);</div><div class="line"><a name="l00408"></a><span class="lineno">  408</span>&#160;  _Xrot.push_back(Xrot);</div><div class="line"><a name="l00409"></a><span class="lineno">  409</span>&#160;  _Ibody.push_back(inertia);</div><div class="line"><a name="l00410"></a><span class="lineno">  410</span>&#160;  _Irot.push_back(rotorInertia);</div><div class="line"><a name="l00411"></a><span class="lineno">  411</span>&#160;  _nDof++;</div><div class="line"><a name="l00412"></a><span class="lineno">  412</span>&#160;</div><div class="line"><a name="l00413"></a><span class="lineno">  413</span>&#160;  addDynamicsVars(1);</div><div class="line"><a name="l00414"></a><span class="lineno">  414</span>&#160;</div><div class="line"><a name="l00415"></a><span class="lineno">  415</span>&#160;  <span class="keywordflow">return</span> _nDof;</div><div class="line"><a name="l00416"></a><span class="lineno">  416</span>&#160;}</div><div class="line"><a name="l00417"></a><span class="lineno">  417</span>&#160;</div><div class="line"><a name="l00431"></a><span class="lineno">  431</span>&#160;<span class="keyword">template</span> &lt;<span class="keyword">typename</span> T&gt;</div><div class="line"><a name="l00432"></a><span class="lineno"><a class="line" href="../../d6/db1/class_floating_base_model.html#a633916e61baece769965271dee7e266f">  432</a></span>&#160;<span class="keywordtype">int</span> <a class="code" href="../../d6/db1/class_floating_base_model.html#adcff6806f39aedf95a3723fc10c6144e">FloatingBaseModel&lt;T&gt;::addBody</a>(<span class="keyword">const</span> <a class="code" href="../../d9/d8e/cpp_types_8h.html#ad7673cd2189f78a761c3a51d9c1314b1">MassProperties&lt;T&gt;</a> &amp;inertia,</div><div class="line"><a name="l00433"></a><span class="lineno">  433</span>&#160;                                  <span class="keyword">const</span> <a class="code" href="../../d9/d8e/cpp_types_8h.html#ad7673cd2189f78a761c3a51d9c1314b1">MassProperties&lt;T&gt;</a> &amp;rotorInertia,</div><div class="line"><a name="l00434"></a><span class="lineno">  434</span>&#160;                                  T gearRatio, <span class="keywordtype">int</span> parent, <a class="code" href="../../de/d1a/namespacespatial.html#acc851583f244f19d9ea07490e9328b51">JointType</a> jointType,</div><div class="line"><a name="l00435"></a><span class="lineno">  435</span>&#160;                                  <a class="code" href="../../da/d9b/namespaceori.html#a02c20a1b1d35988c657aa1e219d609d5">CoordinateAxis</a> jointAxis,</div><div class="line"><a name="l00436"></a><span class="lineno">  436</span>&#160;                                  <span class="keyword">const</span> <a class="code" href="../../d9/d8e/cpp_types_8h.html#ae8ab6bd4079a5317297be5eeb331b1ca">Mat6&lt;T&gt;</a> &amp;Xtree, <span class="keyword">const</span> <a class="code" href="../../d9/d8e/cpp_types_8h.html#ae8ab6bd4079a5317297be5eeb331b1ca">Mat6&lt;T&gt;</a> &amp;Xrot) {</div><div class="line"><a name="l00437"></a><span class="lineno">  437</span>&#160;  <span class="keywordflow">return</span> addBody(<a class="code" href="../../d6/d81/class_spatial_inertia.html">SpatialInertia&lt;T&gt;</a>(inertia), <a class="code" href="../../d6/d81/class_spatial_inertia.html">SpatialInertia&lt;T&gt;</a>(rotorInertia),</div><div class="line"><a name="l00438"></a><span class="lineno">  438</span>&#160;                 gearRatio, parent, jointType, jointAxis, Xtree, Xrot);</div><div class="line"><a name="l00439"></a><span class="lineno">  439</span>&#160;}</div><div class="line"><a name="l00440"></a><span class="lineno">  440</span>&#160;</div><div class="line"><a name="l00441"></a><span class="lineno">  441</span>&#160;<span class="keyword">template</span> &lt;<span class="keyword">typename</span> T&gt;</div><div class="line"><a name="l00442"></a><span class="lineno"><a class="line" href="../../d6/db1/class_floating_base_model.html#af6a72105c1c1ad7565d8dbbfaba401eb">  442</a></span>&#160;<span class="keywordtype">void</span> <a class="code" href="../../d6/db1/class_floating_base_model.html#af6a72105c1c1ad7565d8dbbfaba401eb">FloatingBaseModel&lt;T&gt;::check</a>() {</div><div class="line"><a name="l00443"></a><span class="lineno">  443</span>&#160;  <span class="keywordflow">if</span> (_nDof != _parents.size())</div><div class="line"><a name="l00444"></a><span class="lineno">  444</span>&#160;    <span class="keywordflow">throw</span> std::runtime_error(<span class="stringliteral">&quot;Invalid dof and parents length&quot;</span>);</div><div class="line"><a name="l00445"></a><span class="lineno">  445</span>&#160;}</div><div class="line"><a name="l00446"></a><span class="lineno">  446</span>&#160;</div><div class="line"><a name="l00451"></a><span class="lineno">  451</span>&#160;<span class="keyword">template</span> &lt;<span class="keyword">typename</span> T&gt;</div><div class="line"><a name="l00452"></a><span class="lineno"><a class="line" href="../../d6/db1/class_floating_base_model.html#ac3e58feb2dceedb9739bc76bcaccbc98">  452</a></span>&#160;T <a class="code" href="../../d6/db1/class_floating_base_model.html#ac3e58feb2dceedb9739bc76bcaccbc98">FloatingBaseModel&lt;T&gt;::totalNonRotorMass</a>() {</div><div class="line"><a name="l00453"></a><span class="lineno">  453</span>&#160;  T totalMass = 0;</div><div class="line"><a name="l00454"></a><span class="lineno">  454</span>&#160;  <span class="keywordflow">for</span> (<span class="keywordtype">size_t</span> i = 0; i &lt; _nDof; i++) {</div><div class="line"><a name="l00455"></a><span class="lineno">  455</span>&#160;    totalMass += _Ibody[i].getMass();</div><div class="line"><a name="l00456"></a><span class="lineno">  456</span>&#160;  }</div><div class="line"><a name="l00457"></a><span class="lineno">  457</span>&#160;  <span class="keywordflow">return</span> totalMass;</div><div class="line"><a name="l00458"></a><span class="lineno">  458</span>&#160;}</div><div class="line"><a name="l00459"></a><span class="lineno">  459</span>&#160;</div><div class="line"><a name="l00464"></a><span class="lineno">  464</span>&#160;<span class="keyword">template</span> &lt;<span class="keyword">typename</span> T&gt;</div><div class="line"><a name="l00465"></a><span class="lineno"><a class="line" href="../../d6/db1/class_floating_base_model.html#a76cd0fcd431c77e002d8e111b2880cbe">  465</a></span>&#160;T <a class="code" href="../../d6/db1/class_floating_base_model.html#a76cd0fcd431c77e002d8e111b2880cbe">FloatingBaseModel&lt;T&gt;::totalRotorMass</a>() {</div><div class="line"><a name="l00466"></a><span class="lineno">  466</span>&#160;  T totalMass = 0;</div><div class="line"><a name="l00467"></a><span class="lineno">  467</span>&#160;  <span class="keywordflow">for</span> (<span class="keywordtype">size_t</span> i = 0; i &lt; _nDof; i++) {</div><div class="line"><a name="l00468"></a><span class="lineno">  468</span>&#160;    totalMass += _Irot[i].getMass();</div><div class="line"><a name="l00469"></a><span class="lineno">  469</span>&#160;  }</div><div class="line"><a name="l00470"></a><span class="lineno">  470</span>&#160;  <span class="keywordflow">return</span> totalMass;</div><div class="line"><a name="l00471"></a><span class="lineno">  471</span>&#160;}</div><div class="line"><a name="l00472"></a><span class="lineno">  472</span>&#160;</div><div class="line"><a name="l00478"></a><span class="lineno">  478</span>&#160;<span class="keyword">template</span> &lt;<span class="keyword">typename</span> T&gt;</div><div class="line"><a name="l00479"></a><span class="lineno"><a class="line" href="../../d6/db1/class_floating_base_model.html#a4c4c27d3bc26c3ad1a3306d16b57f085">  479</a></span>&#160;<span class="keywordtype">void</span> <a class="code" href="../../d6/db1/class_floating_base_model.html#a4c4c27d3bc26c3ad1a3306d16b57f085">FloatingBaseModel&lt;T&gt;::forwardKinematics</a>() {</div><div class="line"><a name="l00480"></a><span class="lineno">  480</span>&#160;  <span class="keywordflow">if</span> (_kinematicsUpToDate) <span class="keywordflow">return</span>;</div><div class="line"><a name="l00481"></a><span class="lineno">  481</span>&#160;</div><div class="line"><a name="l00482"></a><span class="lineno">  482</span>&#160;  <span class="comment">// calculate joint transformations</span></div><div class="line"><a name="l00483"></a><span class="lineno">  483</span>&#160;  _Xup[5] = <a class="code" href="../../de/d1a/namespacespatial.html#ae4a2145f3028087511b4f17c0152ec83">createSXform</a>(<a class="code" href="../../da/d9b/namespaceori.html#af88fa91d686c646d41f9b747a6bc90f0">quaternionToRotationMatrix</a>(_state.bodyOrientation),</div><div class="line"><a name="l00484"></a><span class="lineno">  484</span>&#160;                         _state.bodyPosition);</div><div class="line"><a name="l00485"></a><span class="lineno">  485</span>&#160;  _v[5] = _state.bodyVelocity;</div><div class="line"><a name="l00486"></a><span class="lineno">  486</span>&#160;  <span class="keywordflow">for</span> (<span class="keywordtype">size_t</span> i = 6; i &lt; _nDof; i++) {</div><div class="line"><a name="l00487"></a><span class="lineno">  487</span>&#160;    <span class="comment">// joint xform</span></div><div class="line"><a name="l00488"></a><span class="lineno">  488</span>&#160;    <a class="code" href="../../d9/d8e/cpp_types_8h.html#ae8ab6bd4079a5317297be5eeb331b1ca">Mat6&lt;T&gt;</a> XJ = <a class="code" href="../../de/d1a/namespacespatial.html#a6440807f3b1854b1708f49048331811a">jointXform</a>(_jointTypes[i], _jointAxes[i], _state.q[i - 6]);</div><div class="line"><a name="l00489"></a><span class="lineno">  489</span>&#160;    _Xup[i] = XJ * _Xtree[i];</div><div class="line"><a name="l00490"></a><span class="lineno">  490</span>&#160;    _S[i] = jointMotionSubspace&lt;T&gt;(_jointTypes[i], _jointAxes[i]);</div><div class="line"><a name="l00491"></a><span class="lineno">  491</span>&#160;    <a class="code" href="../../d9/d8e/cpp_types_8h.html#a3d70f8eb9cf1855ca989f887a5543acc">SVec&lt;T&gt;</a> vJ = _S[i] * _state.qd[i - 6];</div><div class="line"><a name="l00492"></a><span class="lineno">  492</span>&#160;    <span class="comment">// total velocity of body i</span></div><div class="line"><a name="l00493"></a><span class="lineno">  493</span>&#160;    _v[i] = _Xup[i] * _v[_parents[i]] + vJ;</div><div class="line"><a name="l00494"></a><span class="lineno">  494</span>&#160;</div><div class="line"><a name="l00495"></a><span class="lineno">  495</span>&#160;    <span class="comment">// Same for rotors</span></div><div class="line"><a name="l00496"></a><span class="lineno">  496</span>&#160;    <a class="code" href="../../d9/d8e/cpp_types_8h.html#ae8ab6bd4079a5317297be5eeb331b1ca">Mat6&lt;T&gt;</a> XJrot = <a class="code" href="../../de/d1a/namespacespatial.html#a6440807f3b1854b1708f49048331811a">jointXform</a>(_jointTypes[i], _jointAxes[i],</div><div class="line"><a name="l00497"></a><span class="lineno">  497</span>&#160;                               _state.q[i - 6] * _gearRatios[i]);</div><div class="line"><a name="l00498"></a><span class="lineno">  498</span>&#160;    _Srot[i] = _S[i] * _gearRatios[i];</div><div class="line"><a name="l00499"></a><span class="lineno">  499</span>&#160;    <a class="code" href="../../d9/d8e/cpp_types_8h.html#a3d70f8eb9cf1855ca989f887a5543acc">SVec&lt;T&gt;</a> vJrot = _Srot[i] * _state.qd[i - 6];</div><div class="line"><a name="l00500"></a><span class="lineno">  500</span>&#160;    _Xuprot[i] = XJrot * _Xrot[i];</div><div class="line"><a name="l00501"></a><span class="lineno">  501</span>&#160;    _vrot[i] = _Xuprot[i] * _v[_parents[i]] + vJrot;</div><div class="line"><a name="l00502"></a><span class="lineno">  502</span>&#160;</div><div class="line"><a name="l00503"></a><span class="lineno">  503</span>&#160;    <span class="comment">// Coriolis accelerations</span></div><div class="line"><a name="l00504"></a><span class="lineno">  504</span>&#160;    _c[i] = <a class="code" href="../../de/d1a/namespacespatial.html#a49f73699d3c57d07835e8ab61af3f34a">motionCrossProduct</a>(_v[i], vJ);</div><div class="line"><a name="l00505"></a><span class="lineno">  505</span>&#160;    _crot[i] = <a class="code" href="../../de/d1a/namespacespatial.html#a49f73699d3c57d07835e8ab61af3f34a">motionCrossProduct</a>(_vrot[i], vJrot);</div><div class="line"><a name="l00506"></a><span class="lineno">  506</span>&#160;  }</div><div class="line"><a name="l00507"></a><span class="lineno">  507</span>&#160;</div><div class="line"><a name="l00508"></a><span class="lineno">  508</span>&#160;  <span class="comment">// calculate from absolute transformations</span></div><div class="line"><a name="l00509"></a><span class="lineno">  509</span>&#160;  <span class="keywordflow">for</span> (<span class="keywordtype">size_t</span> i = 5; i &lt; _nDof; i++) {</div><div class="line"><a name="l00510"></a><span class="lineno">  510</span>&#160;    <span class="keywordflow">if</span> (_parents[i] == 0) {</div><div class="line"><a name="l00511"></a><span class="lineno">  511</span>&#160;      _Xa[i] = _Xup[i];  <span class="comment">// float base</span></div><div class="line"><a name="l00512"></a><span class="lineno">  512</span>&#160;    } <span class="keywordflow">else</span> {</div><div class="line"><a name="l00513"></a><span class="lineno">  513</span>&#160;      _Xa[i] = _Xup[i] * _Xa[_parents[i]];</div><div class="line"><a name="l00514"></a><span class="lineno">  514</span>&#160;    }</div><div class="line"><a name="l00515"></a><span class="lineno">  515</span>&#160;  }</div><div class="line"><a name="l00516"></a><span class="lineno">  516</span>&#160;</div><div class="line"><a name="l00517"></a><span class="lineno">  517</span>&#160;  <span class="comment">// ground contact points</span></div><div class="line"><a name="l00518"></a><span class="lineno">  518</span>&#160;  <span class="comment">//  // TODO : we end up inverting the same Xa a few times (like for the 8</span></div><div class="line"><a name="l00519"></a><span class="lineno">  519</span>&#160;  <span class="comment">//  points on the body). this isn&#39;t super efficient.</span></div><div class="line"><a name="l00520"></a><span class="lineno">  520</span>&#160;  <span class="keywordflow">for</span> (<span class="keywordtype">size_t</span> j = 0; j &lt; _nGroundContact; j++) {</div><div class="line"><a name="l00521"></a><span class="lineno">  521</span>&#160;    <span class="keywordflow">if</span> (!_compute_contact_info[j]) <span class="keywordflow">continue</span>;</div><div class="line"><a name="l00522"></a><span class="lineno">  522</span>&#160;    <span class="keywordtype">size_t</span> i = _gcParent.at(j);</div><div class="line"><a name="l00523"></a><span class="lineno">  523</span>&#160;    <a class="code" href="../../d9/d8e/cpp_types_8h.html#ae8ab6bd4079a5317297be5eeb331b1ca">Mat6&lt;T&gt;</a> Xai = <a class="code" href="../../de/d1a/namespacespatial.html#af37757ee8ad4510df5478202a62c9502">invertSXform</a>(_Xa[i]);  <span class="comment">// from link to absolute</span></div><div class="line"><a name="l00524"></a><span class="lineno">  524</span>&#160;    <a class="code" href="../../d9/d8e/cpp_types_8h.html#a3d70f8eb9cf1855ca989f887a5543acc">SVec&lt;T&gt;</a> vSpatial = Xai * _v[i];</div><div class="line"><a name="l00525"></a><span class="lineno">  525</span>&#160;</div><div class="line"><a name="l00526"></a><span class="lineno">  526</span>&#160;    <span class="comment">// foot position in world</span></div><div class="line"><a name="l00527"></a><span class="lineno">  527</span>&#160;    _pGC.at(j) = <a class="code" href="../../de/d1a/namespacespatial.html#ab521121fc7af2392144a78003c100a10">sXFormPoint</a>(Xai, _gcLocation.at(j));</div><div class="line"><a name="l00528"></a><span class="lineno">  528</span>&#160;    _vGC.at(j) = <a class="code" href="../../de/d1a/namespacespatial.html#a95b2db654a989f56df5a8977a5be12d6">spatialToLinearVelocity</a>(vSpatial, _pGC.at(j));</div><div class="line"><a name="l00529"></a><span class="lineno">  529</span>&#160;  }</div><div class="line"><a name="l00530"></a><span class="lineno">  530</span>&#160;  _kinematicsUpToDate = <span class="keyword">true</span>;</div><div class="line"><a name="l00531"></a><span class="lineno">  531</span>&#160;}</div><div class="line"><a name="l00532"></a><span class="lineno">  532</span>&#160;</div><div class="line"><a name="l00533"></a><span class="lineno">  533</span>&#160;<span class="comment">// template &lt;typename T&gt;</span></div><div class="line"><a name="l00534"></a><span class="lineno">  534</span>&#160;<span class="comment">// void FloatingBaseModel&lt;T&gt;::getPositionVelocity(</span></div><div class="line"><a name="l00535"></a><span class="lineno">  535</span>&#160;<span class="comment">// const int &amp; link_idx, const Vec3&lt;T&gt; &amp; local_pos,</span></div><div class="line"><a name="l00536"></a><span class="lineno">  536</span>&#160;<span class="comment">// Vec3&lt;T&gt; &amp; link_pos, Vec3&lt;T&gt; &amp; link_vel) const {</span></div><div class="line"><a name="l00537"></a><span class="lineno">  537</span>&#160;</div><div class="line"><a name="l00538"></a><span class="lineno">  538</span>&#160;<span class="comment">// Mat6&lt;T&gt; Xai = invertSXform(_Xa[link_idx]); // from link to absolute</span></div><div class="line"><a name="l00539"></a><span class="lineno">  539</span>&#160;<span class="comment">// link_pos = sXFormPoint(Xai, local_pos);</span></div><div class="line"><a name="l00540"></a><span class="lineno">  540</span>&#160;<span class="comment">// link_vel = spatialToLinearVelocity(Xai*_v[link_idx], link_pos);</span></div><div class="line"><a name="l00541"></a><span class="lineno">  541</span>&#160;<span class="comment">//}</span></div><div class="line"><a name="l00542"></a><span class="lineno">  542</span>&#160;</div><div class="line"><a name="l00547"></a><span class="lineno">  547</span>&#160;<span class="keyword">template</span> &lt;<span class="keyword">typename</span> T&gt;</div><div class="line"><a name="l00548"></a><span class="lineno"><a class="line" href="../../d6/db1/class_floating_base_model.html#afca11b3f8d117a20990d309666e1a8f3">  548</a></span>&#160;<span class="keywordtype">void</span> <a class="code" href="../../d6/db1/class_floating_base_model.html#afca11b3f8d117a20990d309666e1a8f3">FloatingBaseModel&lt;T&gt;::contactJacobians</a>() {</div><div class="line"><a name="l00549"></a><span class="lineno">  549</span>&#160;  forwardKinematics();</div><div class="line"><a name="l00550"></a><span class="lineno">  550</span>&#160;  biasAccelerations();</div><div class="line"><a name="l00551"></a><span class="lineno">  551</span>&#160;</div><div class="line"><a name="l00552"></a><span class="lineno">  552</span>&#160;  <span class="keywordflow">for</span> (<span class="keywordtype">size_t</span> k = 0; k &lt; _nGroundContact; k++) {</div><div class="line"><a name="l00553"></a><span class="lineno">  553</span>&#160;    _Jc[k].setZero();</div><div class="line"><a name="l00554"></a><span class="lineno">  554</span>&#160;    _Jcdqd[k].setZero();</div><div class="line"><a name="l00555"></a><span class="lineno">  555</span>&#160;</div><div class="line"><a name="l00556"></a><span class="lineno">  556</span>&#160;    <span class="comment">// Skip it if we don&#39;t care about it</span></div><div class="line"><a name="l00557"></a><span class="lineno">  557</span>&#160;    <span class="keywordflow">if</span> (!_compute_contact_info[k]) <span class="keywordflow">continue</span>;</div><div class="line"><a name="l00558"></a><span class="lineno">  558</span>&#160;</div><div class="line"><a name="l00559"></a><span class="lineno">  559</span>&#160;    <span class="keywordtype">size_t</span> i = _gcParent.at(k);</div><div class="line"><a name="l00560"></a><span class="lineno">  560</span>&#160;</div><div class="line"><a name="l00561"></a><span class="lineno">  561</span>&#160;    <span class="comment">// Rotation to absolute coords</span></div><div class="line"><a name="l00562"></a><span class="lineno">  562</span>&#160;    <a class="code" href="../../d9/d8e/cpp_types_8h.html#aa8cb424e643d476ea485238dad7c3e18">Mat3&lt;T&gt;</a> Rai = _Xa[i].template block&lt;3, 3&gt;(0, 0).transpose();</div><div class="line"><a name="l00563"></a><span class="lineno">  563</span>&#160;    <a class="code" href="../../d9/d8e/cpp_types_8h.html#ae8ab6bd4079a5317297be5eeb331b1ca">Mat6&lt;T&gt;</a> Xc = <a class="code" href="../../de/d1a/namespacespatial.html#ae4a2145f3028087511b4f17c0152ec83">createSXform</a>(Rai, _gcLocation.at(k));</div><div class="line"><a name="l00564"></a><span class="lineno">  564</span>&#160;</div><div class="line"><a name="l00565"></a><span class="lineno">  565</span>&#160;    <span class="comment">// Bias acceleration</span></div><div class="line"><a name="l00566"></a><span class="lineno">  566</span>&#160;    <a class="code" href="../../d9/d8e/cpp_types_8h.html#a3d70f8eb9cf1855ca989f887a5543acc">SVec&lt;T&gt;</a> ac = Xc * _avp[i];</div><div class="line"><a name="l00567"></a><span class="lineno">  567</span>&#160;    <a class="code" href="../../d9/d8e/cpp_types_8h.html#a3d70f8eb9cf1855ca989f887a5543acc">SVec&lt;T&gt;</a> vc = Xc * _v[i];</div><div class="line"><a name="l00568"></a><span class="lineno">  568</span>&#160;</div><div class="line"><a name="l00569"></a><span class="lineno">  569</span>&#160;    <span class="comment">// Correct to classical</span></div><div class="line"><a name="l00570"></a><span class="lineno">  570</span>&#160;    _Jcdqd[k] = <a class="code" href="../../de/d1a/namespacespatial.html#a13df9ec1359ecd4150251eaae0895198">spatialToLinearAcceleration</a>(ac, vc);</div><div class="line"><a name="l00571"></a><span class="lineno">  571</span>&#160;</div><div class="line"><a name="l00572"></a><span class="lineno">  572</span>&#160;    <span class="comment">// rows for linear velcoity in the world</span></div><div class="line"><a name="l00573"></a><span class="lineno">  573</span>&#160;    <a class="code" href="../../d9/d8e/cpp_types_8h.html#a190e396ab8cb0eb974ad2a5195edcb24">D3Mat&lt;T&gt;</a> Xout = Xc.template bottomRows&lt;3&gt;();</div><div class="line"><a name="l00574"></a><span class="lineno">  574</span>&#160;</div><div class="line"><a name="l00575"></a><span class="lineno">  575</span>&#160;    <span class="comment">// from tips to base</span></div><div class="line"><a name="l00576"></a><span class="lineno">  576</span>&#160;    <span class="keywordflow">while</span> (i &gt; 5) {</div><div class="line"><a name="l00577"></a><span class="lineno">  577</span>&#160;      _Jc[k].col(i) = Xout * _S[i];</div><div class="line"><a name="l00578"></a><span class="lineno">  578</span>&#160;      Xout = Xout * _Xup[i];</div><div class="line"><a name="l00579"></a><span class="lineno">  579</span>&#160;      i = _parents[i];</div><div class="line"><a name="l00580"></a><span class="lineno">  580</span>&#160;    }</div><div class="line"><a name="l00581"></a><span class="lineno">  581</span>&#160;    _Jc[k].template leftCols&lt;6&gt;() = Xout;</div><div class="line"><a name="l00582"></a><span class="lineno">  582</span>&#160;  }</div><div class="line"><a name="l00583"></a><span class="lineno">  583</span>&#160;}</div><div class="line"><a name="l00584"></a><span class="lineno">  584</span>&#160;</div><div class="line"><a name="l00589"></a><span class="lineno">  589</span>&#160;<span class="keyword">template</span> &lt;<span class="keyword">typename</span> T&gt;</div><div class="line"><a name="l00590"></a><span class="lineno"><a class="line" href="../../d6/db1/class_floating_base_model.html#abd6c4bde8153cace7d96761c7d6c56cc">  590</a></span>&#160;<span class="keywordtype">void</span> <a class="code" href="../../d6/db1/class_floating_base_model.html#abd6c4bde8153cace7d96761c7d6c56cc">FloatingBaseModel&lt;T&gt;::biasAccelerations</a>() {</div><div class="line"><a name="l00591"></a><span class="lineno">  591</span>&#160;  <span class="keywordflow">if</span> (_biasAccelerationsUpToDate) <span class="keywordflow">return</span>;</div><div class="line"><a name="l00592"></a><span class="lineno">  592</span>&#160;  forwardKinematics();</div><div class="line"><a name="l00593"></a><span class="lineno">  593</span>&#160;  <span class="comment">// velocity product acceelration of base</span></div><div class="line"><a name="l00594"></a><span class="lineno">  594</span>&#160;  _avp[5] &lt;&lt; 0, 0, 0, 0, 0, 0;</div><div class="line"><a name="l00595"></a><span class="lineno">  595</span>&#160;</div><div class="line"><a name="l00596"></a><span class="lineno">  596</span>&#160;  <span class="comment">// from base to tips</span></div><div class="line"><a name="l00597"></a><span class="lineno">  597</span>&#160;  <span class="keywordflow">for</span> (<span class="keywordtype">size_t</span> i = 6; i &lt; _nDof; i++) {</div><div class="line"><a name="l00598"></a><span class="lineno">  598</span>&#160;    <span class="comment">// Outward kinamtic propagtion</span></div><div class="line"><a name="l00599"></a><span class="lineno">  599</span>&#160;    _avp[i] = _Xup[i] * _avp[_parents[i]] + _c[i];</div><div class="line"><a name="l00600"></a><span class="lineno">  600</span>&#160;    _avprot[i] = _Xuprot[i] * _avp[_parents[i]] + _crot[i];</div><div class="line"><a name="l00601"></a><span class="lineno">  601</span>&#160;  }</div><div class="line"><a name="l00602"></a><span class="lineno">  602</span>&#160;  _biasAccelerationsUpToDate = <span class="keyword">true</span>;</div><div class="line"><a name="l00603"></a><span class="lineno">  603</span>&#160;}</div><div class="line"><a name="l00604"></a><span class="lineno">  604</span>&#160;</div><div class="line"><a name="l00609"></a><span class="lineno">  609</span>&#160;<span class="keyword">template</span> &lt;<span class="keyword">typename</span> T&gt;</div><div class="line"><a name="l00610"></a><span class="lineno"><a class="line" href="../../d6/db1/class_floating_base_model.html#ae79c032f47bdee3795e4707429a27885">  610</a></span>&#160;<a class="code" href="../../d9/d8e/cpp_types_8h.html#ae2e5a0a178a2c528d8aa939f85db7a3e">DVec&lt;T&gt;</a> <a class="code" href="../../d6/db1/class_floating_base_model.html#ae79c032f47bdee3795e4707429a27885">FloatingBaseModel&lt;T&gt;::generalizedGravityForce</a>() {</div><div class="line"><a name="l00611"></a><span class="lineno">  611</span>&#160;  compositeInertias();</div><div class="line"><a name="l00612"></a><span class="lineno">  612</span>&#160;</div><div class="line"><a name="l00613"></a><span class="lineno">  613</span>&#160;  <a class="code" href="../../d9/d8e/cpp_types_8h.html#a3d70f8eb9cf1855ca989f887a5543acc">SVec&lt;T&gt;</a> aGravity;</div><div class="line"><a name="l00614"></a><span class="lineno">  614</span>&#160;  aGravity &lt;&lt; 0, 0, 0, _gravity[0], _gravity[1], _gravity[2];</div><div class="line"><a name="l00615"></a><span class="lineno">  615</span>&#160;  _ag[5] = _Xup[5] * aGravity;</div><div class="line"><a name="l00616"></a><span class="lineno">  616</span>&#160;</div><div class="line"><a name="l00617"></a><span class="lineno">  617</span>&#160;  <span class="comment">// Gravity comp force is the same as force required to accelerate</span></div><div class="line"><a name="l00618"></a><span class="lineno">  618</span>&#160;  <span class="comment">// oppostite gravity</span></div><div class="line"><a name="l00619"></a><span class="lineno">  619</span>&#160;  _G.template topRows&lt;6&gt;() = -_IC[5].getMatrix() * _ag[5];</div><div class="line"><a name="l00620"></a><span class="lineno">  620</span>&#160;  <span class="keywordflow">for</span> (<span class="keywordtype">size_t</span> i = 6; i &lt; _nDof; i++) {</div><div class="line"><a name="l00621"></a><span class="lineno">  621</span>&#160;    _ag[i] = _Xup[i] * _ag[_parents[i]];</div><div class="line"><a name="l00622"></a><span class="lineno">  622</span>&#160;    _agrot[i] = _Xuprot[i] * _ag[_parents[i]];</div><div class="line"><a name="l00623"></a><span class="lineno">  623</span>&#160;</div><div class="line"><a name="l00624"></a><span class="lineno">  624</span>&#160;    <span class="comment">// body and rotor</span></div><div class="line"><a name="l00625"></a><span class="lineno">  625</span>&#160;    _G[i] = -_S[i].dot(_IC[i].getMatrix() * _ag[i]) -</div><div class="line"><a name="l00626"></a><span class="lineno">  626</span>&#160;            _Srot[i].dot(_Irot[i].getMatrix() * _agrot[i]);</div><div class="line"><a name="l00627"></a><span class="lineno">  627</span>&#160;  }</div><div class="line"><a name="l00628"></a><span class="lineno">  628</span>&#160;  <span class="keywordflow">return</span> _G;</div><div class="line"><a name="l00629"></a><span class="lineno">  629</span>&#160;}</div><div class="line"><a name="l00630"></a><span class="lineno">  630</span>&#160;</div><div class="line"><a name="l00635"></a><span class="lineno">  635</span>&#160;<span class="keyword">template</span> &lt;<span class="keyword">typename</span> T&gt;</div><div class="line"><a name="l00636"></a><span class="lineno"><a class="line" href="../../d6/db1/class_floating_base_model.html#a48ee0323aedafce849ca9dec1ed6d9c0">  636</a></span>&#160;<a class="code" href="../../d9/d8e/cpp_types_8h.html#ae2e5a0a178a2c528d8aa939f85db7a3e">DVec&lt;T&gt;</a> <a class="code" href="../../d6/db1/class_floating_base_model.html#a48ee0323aedafce849ca9dec1ed6d9c0">FloatingBaseModel&lt;T&gt;::generalizedCoriolisForce</a>() {</div><div class="line"><a name="l00637"></a><span class="lineno">  637</span>&#160;  biasAccelerations();</div><div class="line"><a name="l00638"></a><span class="lineno">  638</span>&#160;</div><div class="line"><a name="l00639"></a><span class="lineno">  639</span>&#160;  <span class="comment">// Floating base force</span></div><div class="line"><a name="l00640"></a><span class="lineno">  640</span>&#160;  <a class="code" href="../../d9/d8e/cpp_types_8h.html#ae8ab6bd4079a5317297be5eeb331b1ca">Mat6&lt;T&gt;</a> Ifb = _Ibody[5].getMatrix();</div><div class="line"><a name="l00641"></a><span class="lineno">  641</span>&#160;  <a class="code" href="../../d9/d8e/cpp_types_8h.html#a3d70f8eb9cf1855ca989f887a5543acc">SVec&lt;T&gt;</a> hfb = Ifb * _v[5];</div><div class="line"><a name="l00642"></a><span class="lineno">  642</span>&#160;  _fvp[5] = Ifb * _avp[5] + <a class="code" href="../../de/d1a/namespacespatial.html#a705e44e685fd39dbd6c1a9002ae9a344">forceCrossProduct</a>(_v[5], hfb);</div><div class="line"><a name="l00643"></a><span class="lineno">  643</span>&#160;</div><div class="line"><a name="l00644"></a><span class="lineno">  644</span>&#160;  <span class="keywordflow">for</span> (<span class="keywordtype">size_t</span> i = 6; i &lt; _nDof; i++) {</div><div class="line"><a name="l00645"></a><span class="lineno">  645</span>&#160;    <span class="comment">// Force on body i</span></div><div class="line"><a name="l00646"></a><span class="lineno">  646</span>&#160;    <a class="code" href="../../d9/d8e/cpp_types_8h.html#ae8ab6bd4079a5317297be5eeb331b1ca">Mat6&lt;T&gt;</a> Ii = _Ibody[i].getMatrix();</div><div class="line"><a name="l00647"></a><span class="lineno">  647</span>&#160;    <a class="code" href="../../d9/d8e/cpp_types_8h.html#a3d70f8eb9cf1855ca989f887a5543acc">SVec&lt;T&gt;</a> hi = Ii * _v[i];</div><div class="line"><a name="l00648"></a><span class="lineno">  648</span>&#160;    _fvp[i] = Ii * _avp[i] + <a class="code" href="../../de/d1a/namespacespatial.html#a705e44e685fd39dbd6c1a9002ae9a344">forceCrossProduct</a>(_v[i], hi);</div><div class="line"><a name="l00649"></a><span class="lineno">  649</span>&#160;</div><div class="line"><a name="l00650"></a><span class="lineno">  650</span>&#160;    <span class="comment">// Force on rotor i</span></div><div class="line"><a name="l00651"></a><span class="lineno">  651</span>&#160;    <a class="code" href="../../d9/d8e/cpp_types_8h.html#ae8ab6bd4079a5317297be5eeb331b1ca">Mat6&lt;T&gt;</a> Ir = _Irot[i].getMatrix();</div><div class="line"><a name="l00652"></a><span class="lineno">  652</span>&#160;    <a class="code" href="../../d9/d8e/cpp_types_8h.html#a3d70f8eb9cf1855ca989f887a5543acc">SVec&lt;T&gt;</a> hr = Ir * _vrot[i];</div><div class="line"><a name="l00653"></a><span class="lineno">  653</span>&#160;    _fvprot[i] = Ir * _avprot[i] + <a class="code" href="../../de/d1a/namespacespatial.html#a705e44e685fd39dbd6c1a9002ae9a344">forceCrossProduct</a>(_vrot[i], hr);</div><div class="line"><a name="l00654"></a><span class="lineno">  654</span>&#160;  }</div><div class="line"><a name="l00655"></a><span class="lineno">  655</span>&#160;</div><div class="line"><a name="l00656"></a><span class="lineno">  656</span>&#160;  <span class="keywordflow">for</span> (<span class="keywordtype">size_t</span> i = _nDof - 1; i &gt; 5; i--) {</div><div class="line"><a name="l00657"></a><span class="lineno">  657</span>&#160;    <span class="comment">// Extract force along the joints</span></div><div class="line"><a name="l00658"></a><span class="lineno">  658</span>&#160;    _Cqd[i] = _S[i].dot(_fvp[i]) + _Srot[i].dot(_fvprot[i]);</div><div class="line"><a name="l00659"></a><span class="lineno">  659</span>&#160;</div><div class="line"><a name="l00660"></a><span class="lineno">  660</span>&#160;    <span class="comment">// Propage force down the tree</span></div><div class="line"><a name="l00661"></a><span class="lineno">  661</span>&#160;    _fvp[_parents[i]] += _Xup[i].transpose() * _fvp[i];</div><div class="line"><a name="l00662"></a><span class="lineno">  662</span>&#160;    _fvp[_parents[i]] += _Xuprot[i].transpose() * _fvprot[i];</div><div class="line"><a name="l00663"></a><span class="lineno">  663</span>&#160;  }</div><div class="line"><a name="l00664"></a><span class="lineno">  664</span>&#160;</div><div class="line"><a name="l00665"></a><span class="lineno">  665</span>&#160;  <span class="comment">// Force on floating base</span></div><div class="line"><a name="l00666"></a><span class="lineno">  666</span>&#160;  _Cqd.template topRows&lt;6&gt;() = _fvp[5];</div><div class="line"><a name="l00667"></a><span class="lineno">  667</span>&#160;  <span class="keywordflow">return</span> _Cqd;</div><div class="line"><a name="l00668"></a><span class="lineno">  668</span>&#160;}</div><div class="line"><a name="l00669"></a><span class="lineno">  669</span>&#160;</div><div class="line"><a name="l00670"></a><span class="lineno">  670</span>&#160;<span class="keyword">template</span> &lt;<span class="keyword">typename</span> T&gt;</div><div class="line"><a name="l00671"></a><span class="lineno"><a class="line" href="../../d6/db1/class_floating_base_model.html#a72dd957e465044ebd8f007385bdf6b29">  671</a></span>&#160;<a class="code" href="../../d9/d8e/cpp_types_8h.html#aa8cb424e643d476ea485238dad7c3e18">Mat3&lt;T&gt;</a> <a class="code" href="../../d6/db1/class_floating_base_model.html#a72dd957e465044ebd8f007385bdf6b29">FloatingBaseModel&lt;T&gt;::getOrientation</a>(<span class="keywordtype">int</span> link_idx) {</div><div class="line"><a name="l00672"></a><span class="lineno">  672</span>&#160;  forwardKinematics();</div><div class="line"><a name="l00673"></a><span class="lineno">  673</span>&#160;  <a class="code" href="../../d9/d8e/cpp_types_8h.html#aa8cb424e643d476ea485238dad7c3e18">Mat3&lt;T&gt;</a> Rai = _Xa[link_idx].template block&lt;3, 3&gt;(0, 0);</div><div class="line"><a name="l00674"></a><span class="lineno">  674</span>&#160;  Rai.transposeInPlace();</div><div class="line"><a name="l00675"></a><span class="lineno">  675</span>&#160;  <span class="keywordflow">return</span> Rai;</div><div class="line"><a name="l00676"></a><span class="lineno">  676</span>&#160;}</div><div class="line"><a name="l00677"></a><span class="lineno">  677</span>&#160;</div><div class="line"><a name="l00678"></a><span class="lineno">  678</span>&#160;</div><div class="line"><a name="l00679"></a><span class="lineno">  679</span>&#160;<span class="keyword">template</span> &lt;<span class="keyword">typename</span> T&gt;</div><div class="line"><a name="l00680"></a><span class="lineno"><a class="line" href="../../d6/db1/class_floating_base_model.html#a6a46aad02aba5eae6cb0a6cda14780ce">  680</a></span>&#160;<a class="code" href="../../d9/d8e/cpp_types_8h.html#a6f07728c866b15ff0676a9b2ebfd2ba6">Vec3&lt;T&gt;</a> <a class="code" href="../../d6/db1/class_floating_base_model.html#a701f7e9f31622845a4a49b30b0846d55">FloatingBaseModel&lt;T&gt;::getPosition</a>(<span class="keyword">const</span> <span class="keywordtype">int</span> link_idx)</div><div class="line"><a name="l00681"></a><span class="lineno">  681</span>&#160;{</div><div class="line"><a name="l00682"></a><span class="lineno">  682</span>&#160;  forwardKinematics();</div><div class="line"><a name="l00683"></a><span class="lineno">  683</span>&#160;  <a class="code" href="../../d9/d8e/cpp_types_8h.html#ae8ab6bd4079a5317297be5eeb331b1ca">Mat6&lt;T&gt;</a> Xai = <a class="code" href="../../de/d1a/namespacespatial.html#af37757ee8ad4510df5478202a62c9502">invertSXform</a>(_Xa[link_idx]); <span class="comment">// from link to absolute</span></div><div class="line"><a name="l00684"></a><span class="lineno">  684</span>&#160;  <a class="code" href="../../d9/d8e/cpp_types_8h.html#a6f07728c866b15ff0676a9b2ebfd2ba6">Vec3&lt;T&gt;</a> link_pos = <a class="code" href="../../de/d1a/namespacespatial.html#ab521121fc7af2392144a78003c100a10">sXFormPoint</a>(Xai, <a class="code" href="../../d9/d8e/cpp_types_8h.html#a6f07728c866b15ff0676a9b2ebfd2ba6">Vec3&lt;T&gt;::Zero</a>());</div><div class="line"><a name="l00685"></a><span class="lineno">  685</span>&#160;  <span class="keywordflow">return</span> link_pos;</div><div class="line"><a name="l00686"></a><span class="lineno">  686</span>&#160;}</div><div class="line"><a name="l00687"></a><span class="lineno">  687</span>&#160;</div><div class="line"><a name="l00688"></a><span class="lineno">  688</span>&#160;<span class="keyword">template</span> &lt;<span class="keyword">typename</span> T&gt;</div><div class="line"><a name="l00689"></a><span class="lineno"><a class="line" href="../../d6/db1/class_floating_base_model.html#a701f7e9f31622845a4a49b30b0846d55">  689</a></span>&#160;<a class="code" href="../../d9/d8e/cpp_types_8h.html#a6f07728c866b15ff0676a9b2ebfd2ba6">Vec3&lt;T&gt;</a> <a class="code" href="../../d6/db1/class_floating_base_model.html#a701f7e9f31622845a4a49b30b0846d55">FloatingBaseModel&lt;T&gt;::getPosition</a>(<span class="keyword">const</span> <span class="keywordtype">int</span> link_idx, <span class="keyword">const</span> <a class="code" href="../../d9/d8e/cpp_types_8h.html#a6f07728c866b15ff0676a9b2ebfd2ba6">Vec3&lt;T&gt;</a> &amp; local_pos)</div><div class="line"><a name="l00690"></a><span class="lineno">  690</span>&#160;{</div><div class="line"><a name="l00691"></a><span class="lineno">  691</span>&#160;  forwardKinematics();</div><div class="line"><a name="l00692"></a><span class="lineno">  692</span>&#160;  <a class="code" href="../../d9/d8e/cpp_types_8h.html#ae8ab6bd4079a5317297be5eeb331b1ca">Mat6&lt;T&gt;</a> Xai = <a class="code" href="../../de/d1a/namespacespatial.html#af37757ee8ad4510df5478202a62c9502">invertSXform</a>(_Xa[link_idx]); <span class="comment">// from link to absolute</span></div><div class="line"><a name="l00693"></a><span class="lineno">  693</span>&#160;  <a class="code" href="../../d9/d8e/cpp_types_8h.html#a6f07728c866b15ff0676a9b2ebfd2ba6">Vec3&lt;T&gt;</a> link_pos = <a class="code" href="../../de/d1a/namespacespatial.html#ab521121fc7af2392144a78003c100a10">sXFormPoint</a>(Xai, local_pos);</div><div class="line"><a name="l00694"></a><span class="lineno">  694</span>&#160;  <span class="keywordflow">return</span> link_pos;</div><div class="line"><a name="l00695"></a><span class="lineno">  695</span>&#160;}</div><div class="line"><a name="l00696"></a><span class="lineno">  696</span>&#160;</div><div class="line"><a name="l00697"></a><span class="lineno">  697</span>&#160;<span class="keyword">template</span> &lt;<span class="keyword">typename</span> T&gt;</div><div class="line"><a name="l00698"></a><span class="lineno"><a class="line" href="../../d6/db1/class_floating_base_model.html#a79fe9f46e0e6617b6e68a6f267f877dc">  698</a></span>&#160;<a class="code" href="../../d9/d8e/cpp_types_8h.html#a6f07728c866b15ff0676a9b2ebfd2ba6">Vec3&lt;T&gt;</a> <a class="code" href="../../d6/db1/class_floating_base_model.html#a79fe9f46e0e6617b6e68a6f267f877dc">FloatingBaseModel&lt;T&gt;::getLinearAcceleration</a>(<span class="keyword">const</span> <span class="keywordtype">int</span> link_idx,</div><div class="line"><a name="l00699"></a><span class="lineno">  699</span>&#160;                                                    <span class="keyword">const</span> <a class="code" href="../../d9/d8e/cpp_types_8h.html#a6f07728c866b15ff0676a9b2ebfd2ba6">Vec3&lt;T&gt;</a> &amp;point) {</div><div class="line"><a name="l00700"></a><span class="lineno">  700</span>&#160;  forwardAccelerationKinematics();</div><div class="line"><a name="l00701"></a><span class="lineno">  701</span>&#160;  <a class="code" href="../../d9/d8e/cpp_types_8h.html#aa8cb424e643d476ea485238dad7c3e18">Mat3&lt;T&gt;</a> R = getOrientation(link_idx);</div><div class="line"><a name="l00702"></a><span class="lineno">  702</span>&#160;  <span class="keywordflow">return</span> R * <a class="code" href="../../de/d1a/namespacespatial.html#a13df9ec1359ecd4150251eaae0895198">spatialToLinearAcceleration</a>(_a[link_idx], _v[link_idx], point);</div><div class="line"><a name="l00703"></a><span class="lineno">  703</span>&#160;}</div><div class="line"><a name="l00704"></a><span class="lineno">  704</span>&#160;</div><div class="line"><a name="l00705"></a><span class="lineno">  705</span>&#160;<span class="keyword">template</span> &lt;<span class="keyword">typename</span> T&gt;</div><div class="line"><a name="l00706"></a><span class="lineno"><a class="line" href="../../d6/db1/class_floating_base_model.html#ab82859c2d81cdf70f26bfdef9eb12e45">  706</a></span>&#160;<a class="code" href="../../d9/d8e/cpp_types_8h.html#a6f07728c866b15ff0676a9b2ebfd2ba6">Vec3&lt;T&gt;</a> <a class="code" href="../../d6/db1/class_floating_base_model.html#a79fe9f46e0e6617b6e68a6f267f877dc">FloatingBaseModel&lt;T&gt;::getLinearAcceleration</a>(<span class="keyword">const</span> <span class="keywordtype">int</span> link_idx) {</div><div class="line"><a name="l00707"></a><span class="lineno">  707</span>&#160;  forwardAccelerationKinematics();</div><div class="line"><a name="l00708"></a><span class="lineno">  708</span>&#160;  <a class="code" href="../../d9/d8e/cpp_types_8h.html#aa8cb424e643d476ea485238dad7c3e18">Mat3&lt;T&gt;</a> R = getOrientation(link_idx);</div><div class="line"><a name="l00709"></a><span class="lineno">  709</span>&#160;  <span class="keywordflow">return</span> R * <a class="code" href="../../de/d1a/namespacespatial.html#a13df9ec1359ecd4150251eaae0895198">spatialToLinearAcceleration</a>(_a[link_idx], _v[link_idx], <a class="code" href="../../d9/d8e/cpp_types_8h.html#a6f07728c866b15ff0676a9b2ebfd2ba6">Vec3&lt;T&gt;::Zero</a>());</div><div class="line"><a name="l00710"></a><span class="lineno">  710</span>&#160;}</div><div class="line"><a name="l00711"></a><span class="lineno">  711</span>&#160;</div><div class="line"><a name="l00712"></a><span class="lineno">  712</span>&#160;</div><div class="line"><a name="l00713"></a><span class="lineno">  713</span>&#160;<span class="keyword">template</span> &lt;<span class="keyword">typename</span> T&gt;</div><div class="line"><a name="l00714"></a><span class="lineno"><a class="line" href="../../d6/db1/class_floating_base_model.html#ac4097f466bc8a392a88273e33c17bb50">  714</a></span>&#160;<a class="code" href="../../d9/d8e/cpp_types_8h.html#a6f07728c866b15ff0676a9b2ebfd2ba6">Vec3&lt;T&gt;</a> <a class="code" href="../../d6/db1/class_floating_base_model.html#ac4097f466bc8a392a88273e33c17bb50">FloatingBaseModel&lt;T&gt;::getLinearVelocity</a>(<span class="keyword">const</span> <span class="keywordtype">int</span> link_idx,</div><div class="line"><a name="l00715"></a><span class="lineno">  715</span>&#160;                                                <span class="keyword">const</span> <a class="code" href="../../d9/d8e/cpp_types_8h.html#a6f07728c866b15ff0676a9b2ebfd2ba6">Vec3&lt;T&gt;</a> &amp;point) {</div><div class="line"><a name="l00716"></a><span class="lineno">  716</span>&#160;  forwardKinematics();</div><div class="line"><a name="l00717"></a><span class="lineno">  717</span>&#160;  <a class="code" href="../../d9/d8e/cpp_types_8h.html#aa8cb424e643d476ea485238dad7c3e18">Mat3&lt;T&gt;</a> Rai = getOrientation(link_idx);</div><div class="line"><a name="l00718"></a><span class="lineno">  718</span>&#160;  <span class="keywordflow">return</span> Rai * <a class="code" href="../../de/d1a/namespacespatial.html#a95b2db654a989f56df5a8977a5be12d6">spatialToLinearVelocity</a>(_v[link_idx], point);</div><div class="line"><a name="l00719"></a><span class="lineno">  719</span>&#160;}</div><div class="line"><a name="l00720"></a><span class="lineno">  720</span>&#160;</div><div class="line"><a name="l00721"></a><span class="lineno">  721</span>&#160;<span class="keyword">template</span> &lt;<span class="keyword">typename</span> T&gt;</div><div class="line"><a name="l00722"></a><span class="lineno"><a class="line" href="../../d6/db1/class_floating_base_model.html#a9d82b1121cc0d526d39fd27ef310ce0c">  722</a></span>&#160;<a class="code" href="../../d9/d8e/cpp_types_8h.html#a6f07728c866b15ff0676a9b2ebfd2ba6">Vec3&lt;T&gt;</a> <a class="code" href="../../d6/db1/class_floating_base_model.html#ac4097f466bc8a392a88273e33c17bb50">FloatingBaseModel&lt;T&gt;::getLinearVelocity</a>(<span class="keyword">const</span> <span class="keywordtype">int</span> link_idx) {</div><div class="line"><a name="l00723"></a><span class="lineno">  723</span>&#160;  forwardKinematics();</div><div class="line"><a name="l00724"></a><span class="lineno">  724</span>&#160;  <a class="code" href="../../d9/d8e/cpp_types_8h.html#aa8cb424e643d476ea485238dad7c3e18">Mat3&lt;T&gt;</a> Rai = getOrientation(link_idx);</div><div class="line"><a name="l00725"></a><span class="lineno">  725</span>&#160;  <span class="keywordflow">return</span> Rai * <a class="code" href="../../de/d1a/namespacespatial.html#a95b2db654a989f56df5a8977a5be12d6">spatialToLinearVelocity</a>(_v[link_idx], <a class="code" href="../../d9/d8e/cpp_types_8h.html#a6f07728c866b15ff0676a9b2ebfd2ba6">Vec3&lt;T&gt;::Zero</a>());</div><div class="line"><a name="l00726"></a><span class="lineno">  726</span>&#160;}</div><div class="line"><a name="l00727"></a><span class="lineno">  727</span>&#160;</div><div class="line"><a name="l00728"></a><span class="lineno">  728</span>&#160;</div><div class="line"><a name="l00729"></a><span class="lineno">  729</span>&#160;</div><div class="line"><a name="l00730"></a><span class="lineno">  730</span>&#160;<span class="keyword">template</span> &lt;<span class="keyword">typename</span> T&gt;</div><div class="line"><a name="l00731"></a><span class="lineno"><a class="line" href="../../d6/db1/class_floating_base_model.html#a33ff105654083ed8c395097fd208b174">  731</a></span>&#160;<a class="code" href="../../d9/d8e/cpp_types_8h.html#a6f07728c866b15ff0676a9b2ebfd2ba6">Vec3&lt;T&gt;</a> <a class="code" href="../../d6/db1/class_floating_base_model.html#a33ff105654083ed8c395097fd208b174">FloatingBaseModel&lt;T&gt;::getAngularVelocity</a>(<span class="keyword">const</span> <span class="keywordtype">int</span> link_idx) {</div><div class="line"><a name="l00732"></a><span class="lineno">  732</span>&#160;  forwardKinematics();</div><div class="line"><a name="l00733"></a><span class="lineno">  733</span>&#160;  <a class="code" href="../../d9/d8e/cpp_types_8h.html#aa8cb424e643d476ea485238dad7c3e18">Mat3&lt;T&gt;</a> Rai = getOrientation(link_idx);</div><div class="line"><a name="l00734"></a><span class="lineno">  734</span>&#160;  <span class="comment">// Vec3&lt;T&gt; v3 =</span></div><div class="line"><a name="l00735"></a><span class="lineno">  735</span>&#160;  <span class="keywordflow">return</span> Rai * _v[link_idx].template head&lt;3&gt;();</div><div class="line"><a name="l00736"></a><span class="lineno">  736</span>&#160;  ;</div><div class="line"><a name="l00737"></a><span class="lineno">  737</span>&#160;}</div><div class="line"><a name="l00738"></a><span class="lineno">  738</span>&#160;</div><div class="line"><a name="l00739"></a><span class="lineno">  739</span>&#160;<span class="keyword">template</span> &lt;<span class="keyword">typename</span> T&gt;</div><div class="line"><a name="l00740"></a><span class="lineno"><a class="line" href="../../d6/db1/class_floating_base_model.html#a5247d4939f30a11b61bebdaa3e55ce5b">  740</a></span>&#160;<a class="code" href="../../d9/d8e/cpp_types_8h.html#a6f07728c866b15ff0676a9b2ebfd2ba6">Vec3&lt;T&gt;</a> <a class="code" href="../../d6/db1/class_floating_base_model.html#a5247d4939f30a11b61bebdaa3e55ce5b">FloatingBaseModel&lt;T&gt;::getAngularAcceleration</a>(<span class="keyword">const</span> <span class="keywordtype">int</span> link_idx) {</div><div class="line"><a name="l00741"></a><span class="lineno">  741</span>&#160;  forwardAccelerationKinematics();</div><div class="line"><a name="l00742"></a><span class="lineno">  742</span>&#160;  <a class="code" href="../../d9/d8e/cpp_types_8h.html#aa8cb424e643d476ea485238dad7c3e18">Mat3&lt;T&gt;</a> Rai = getOrientation(link_idx);</div><div class="line"><a name="l00743"></a><span class="lineno">  743</span>&#160;  <span class="keywordflow">return</span> Rai * _a[link_idx].template head&lt;3&gt;();</div><div class="line"><a name="l00744"></a><span class="lineno">  744</span>&#160;}</div><div class="line"><a name="l00745"></a><span class="lineno">  745</span>&#160;</div><div class="line"><a name="l00752"></a><span class="lineno">  752</span>&#160;<span class="keyword">template</span> &lt;<span class="keyword">typename</span> T&gt;</div><div class="line"><a name="l00753"></a><span class="lineno"><a class="line" href="../../d6/db1/class_floating_base_model.html#a4ecf73ee1e17247d25f80b33c07da0fd">  753</a></span>&#160;<span class="keywordtype">void</span> <a class="code" href="../../d6/db1/class_floating_base_model.html#a4ecf73ee1e17247d25f80b33c07da0fd">FloatingBaseModel&lt;T&gt;::compositeInertias</a>() {</div><div class="line"><a name="l00754"></a><span class="lineno">  754</span>&#160;  <span class="keywordflow">if</span> (_compositeInertiasUpToDate) <span class="keywordflow">return</span>;</div><div class="line"><a name="l00755"></a><span class="lineno">  755</span>&#160;</div><div class="line"><a name="l00756"></a><span class="lineno">  756</span>&#160;  forwardKinematics();</div><div class="line"><a name="l00757"></a><span class="lineno">  757</span>&#160;  <span class="comment">// initialize</span></div><div class="line"><a name="l00758"></a><span class="lineno">  758</span>&#160;  <span class="keywordflow">for</span> (<span class="keywordtype">size_t</span> i = 5; i &lt; _nDof; i++) {</div><div class="line"><a name="l00759"></a><span class="lineno">  759</span>&#160;    _IC[i].setMatrix(_Ibody[i].getMatrix());</div><div class="line"><a name="l00760"></a><span class="lineno">  760</span>&#160;  }</div><div class="line"><a name="l00761"></a><span class="lineno">  761</span>&#160;</div><div class="line"><a name="l00762"></a><span class="lineno">  762</span>&#160;  <span class="comment">// backward loop</span></div><div class="line"><a name="l00763"></a><span class="lineno">  763</span>&#160;  <span class="keywordflow">for</span> (<span class="keywordtype">size_t</span> i = _nDof - 1; i &gt; 5; i--) {</div><div class="line"><a name="l00764"></a><span class="lineno">  764</span>&#160;    <span class="comment">// Propagate inertia down the tree</span></div><div class="line"><a name="l00765"></a><span class="lineno">  765</span>&#160;    _IC[_parents[i]].addMatrix(_Xup[i].transpose() * _IC[i].getMatrix() *</div><div class="line"><a name="l00766"></a><span class="lineno">  766</span>&#160;                               _Xup[i]);</div><div class="line"><a name="l00767"></a><span class="lineno">  767</span>&#160;    _IC[_parents[i]].addMatrix(_Xuprot[i].transpose() * _Irot[i].getMatrix() *</div><div class="line"><a name="l00768"></a><span class="lineno">  768</span>&#160;                               _Xuprot[i]);</div><div class="line"><a name="l00769"></a><span class="lineno">  769</span>&#160;  }</div><div class="line"><a name="l00770"></a><span class="lineno">  770</span>&#160;  _compositeInertiasUpToDate = <span class="keyword">true</span>;</div><div class="line"><a name="l00771"></a><span class="lineno">  771</span>&#160;}</div><div class="line"><a name="l00772"></a><span class="lineno">  772</span>&#160;</div><div class="line"><a name="l00777"></a><span class="lineno">  777</span>&#160;<span class="keyword">template</span> &lt;<span class="keyword">typename</span> T&gt;</div><div class="line"><a name="l00778"></a><span class="lineno"><a class="line" href="../../d6/db1/class_floating_base_model.html#a73d58fada538f7d74ad94c48e5024b78">  778</a></span>&#160;<a class="code" href="../../d9/d8e/cpp_types_8h.html#a807200f3771dbd2cb2e4be52c5f2518c">DMat&lt;T&gt;</a> <a class="code" href="../../d6/db1/class_floating_base_model.html#a73d58fada538f7d74ad94c48e5024b78">FloatingBaseModel&lt;T&gt;::massMatrix</a>() {</div><div class="line"><a name="l00779"></a><span class="lineno">  779</span>&#160;  compositeInertias();</div><div class="line"><a name="l00780"></a><span class="lineno">  780</span>&#160;  _H.setZero();</div><div class="line"><a name="l00781"></a><span class="lineno">  781</span>&#160;</div><div class="line"><a name="l00782"></a><span class="lineno">  782</span>&#160;  <span class="comment">// Top left corner is the locked inertia of the whole system</span></div><div class="line"><a name="l00783"></a><span class="lineno">  783</span>&#160;  _H.template topLeftCorner&lt;6, 6&gt;() = _IC[5].getMatrix();</div><div class="line"><a name="l00784"></a><span class="lineno">  784</span>&#160;</div><div class="line"><a name="l00785"></a><span class="lineno">  785</span>&#160;  <span class="keywordflow">for</span> (<span class="keywordtype">size_t</span> j = 6; j &lt; _nDof; j++) {</div><div class="line"><a name="l00786"></a><span class="lineno">  786</span>&#160;    <span class="comment">// f = spatial force required for a unit qdd_j</span></div><div class="line"><a name="l00787"></a><span class="lineno">  787</span>&#160;    <a class="code" href="../../d9/d8e/cpp_types_8h.html#a3d70f8eb9cf1855ca989f887a5543acc">SVec&lt;T&gt;</a> <a class="code" href="../../dc/dd7/test__casadi_8cpp.html#ae20d7fba59865a21504b65cdb44cbfe5">f</a> = _IC[j].getMatrix() * _S[j];</div><div class="line"><a name="l00788"></a><span class="lineno">  788</span>&#160;    <a class="code" href="../../d9/d8e/cpp_types_8h.html#a3d70f8eb9cf1855ca989f887a5543acc">SVec&lt;T&gt;</a> frot = _Irot[j].getMatrix() * _Srot[j];</div><div class="line"><a name="l00789"></a><span class="lineno">  789</span>&#160;</div><div class="line"><a name="l00790"></a><span class="lineno">  790</span>&#160;    _H(j, j) = _S[j].dot(f) + _Srot[j].dot(frot);</div><div class="line"><a name="l00791"></a><span class="lineno">  791</span>&#160;</div><div class="line"><a name="l00792"></a><span class="lineno">  792</span>&#160;    <span class="comment">// Propagate down the tree</span></div><div class="line"><a name="l00793"></a><span class="lineno">  793</span>&#160;    f = _Xup[j].transpose() * f + _Xuprot[j].transpose() * frot;</div><div class="line"><a name="l00794"></a><span class="lineno">  794</span>&#160;    <span class="keywordtype">size_t</span> i = _parents[j];</div><div class="line"><a name="l00795"></a><span class="lineno">  795</span>&#160;    <span class="keywordflow">while</span> (i &gt; 5) {</div><div class="line"><a name="l00796"></a><span class="lineno">  796</span>&#160;      <span class="comment">// in here f is expressed in frame {i}</span></div><div class="line"><a name="l00797"></a><span class="lineno">  797</span>&#160;      _H(i, j) = _S[i].dot(f);</div><div class="line"><a name="l00798"></a><span class="lineno">  798</span>&#160;      _H(j, i) = _H(i, j);</div><div class="line"><a name="l00799"></a><span class="lineno">  799</span>&#160;</div><div class="line"><a name="l00800"></a><span class="lineno">  800</span>&#160;      <span class="comment">// Propagate down the tree</span></div><div class="line"><a name="l00801"></a><span class="lineno">  801</span>&#160;      f = _Xup[i].transpose() * <a class="code" href="../../dc/dd7/test__casadi_8cpp.html#ae20d7fba59865a21504b65cdb44cbfe5">f</a>;</div><div class="line"><a name="l00802"></a><span class="lineno">  802</span>&#160;      i = _parents[i];</div><div class="line"><a name="l00803"></a><span class="lineno">  803</span>&#160;    }</div><div class="line"><a name="l00804"></a><span class="lineno">  804</span>&#160;</div><div class="line"><a name="l00805"></a><span class="lineno">  805</span>&#160;    <span class="comment">// Force on floating base</span></div><div class="line"><a name="l00806"></a><span class="lineno">  806</span>&#160;    _H.template block&lt;6, 1&gt;(0, j) = f;</div><div class="line"><a name="l00807"></a><span class="lineno">  807</span>&#160;    _H.template block&lt;1, 6&gt;(j, 0) = f.adjoint();</div><div class="line"><a name="l00808"></a><span class="lineno">  808</span>&#160;  }</div><div class="line"><a name="l00809"></a><span class="lineno">  809</span>&#160;  <span class="keywordflow">return</span> _H;</div><div class="line"><a name="l00810"></a><span class="lineno">  810</span>&#160;}</div><div class="line"><a name="l00811"></a><span class="lineno">  811</span>&#160;</div><div class="line"><a name="l00812"></a><span class="lineno">  812</span>&#160;<span class="keyword">template</span> &lt;<span class="keyword">typename</span> T&gt;</div><div class="line"><a name="l00813"></a><span class="lineno"><a class="line" href="../../d6/db1/class_floating_base_model.html#a8d1ea8345adf3034553a1a7f6646bd5e">  813</a></span>&#160;<span class="keywordtype">void</span> <a class="code" href="../../d6/db1/class_floating_base_model.html#a8d1ea8345adf3034553a1a7f6646bd5e">FloatingBaseModel&lt;T&gt;::forwardAccelerationKinematics</a>() {</div><div class="line"><a name="l00814"></a><span class="lineno">  814</span>&#160;  <span class="keywordflow">if</span> (_accelerationsUpToDate) {</div><div class="line"><a name="l00815"></a><span class="lineno">  815</span>&#160;    <span class="keywordflow">return</span>;</div><div class="line"><a name="l00816"></a><span class="lineno">  816</span>&#160;  }</div><div class="line"><a name="l00817"></a><span class="lineno">  817</span>&#160;</div><div class="line"><a name="l00818"></a><span class="lineno">  818</span>&#160;  forwardKinematics();</div><div class="line"><a name="l00819"></a><span class="lineno">  819</span>&#160;  biasAccelerations();</div><div class="line"><a name="l00820"></a><span class="lineno">  820</span>&#160;</div><div class="line"><a name="l00821"></a><span class="lineno">  821</span>&#160;  <span class="comment">// Initialize gravity with model info</span></div><div class="line"><a name="l00822"></a><span class="lineno">  822</span>&#160;  <a class="code" href="../../d9/d8e/cpp_types_8h.html#a3d70f8eb9cf1855ca989f887a5543acc">SVec&lt;T&gt;</a> aGravity = <a class="code" href="../../d9/d8e/cpp_types_8h.html#a3d70f8eb9cf1855ca989f887a5543acc">SVec&lt;T&gt;::Zero</a>();</div><div class="line"><a name="l00823"></a><span class="lineno">  823</span>&#160;  aGravity.template tail&lt;3&gt;() = _gravity;</div><div class="line"><a name="l00824"></a><span class="lineno">  824</span>&#160;</div><div class="line"><a name="l00825"></a><span class="lineno">  825</span>&#160;  <span class="comment">// Spatial force for floating base</span></div><div class="line"><a name="l00826"></a><span class="lineno">  826</span>&#160;  _a[5] = -_Xup[5] * aGravity + _dState.dBodyVelocity;</div><div class="line"><a name="l00827"></a><span class="lineno">  827</span>&#160;</div><div class="line"><a name="l00828"></a><span class="lineno">  828</span>&#160;  <span class="comment">// loop through joints</span></div><div class="line"><a name="l00829"></a><span class="lineno">  829</span>&#160;  <span class="keywordflow">for</span> (<span class="keywordtype">size_t</span> i = 6; i &lt; _nDof; i++) {</div><div class="line"><a name="l00830"></a><span class="lineno">  830</span>&#160;    <span class="comment">// spatial acceleration</span></div><div class="line"><a name="l00831"></a><span class="lineno">  831</span>&#160;    _a[i] = _Xup[i] * _a[_parents[i]] + _S[i] * _dState.qdd[i - 6] + _c[i];</div><div class="line"><a name="l00832"></a><span class="lineno">  832</span>&#160;    _arot[i] =</div><div class="line"><a name="l00833"></a><span class="lineno">  833</span>&#160;        _Xuprot[i] * _a[_parents[i]] + _Srot[i] * _dState.qdd[i - 6] + _crot[i];</div><div class="line"><a name="l00834"></a><span class="lineno">  834</span>&#160;  }</div><div class="line"><a name="l00835"></a><span class="lineno">  835</span>&#160;  _accelerationsUpToDate = <span class="keyword">true</span>;</div><div class="line"><a name="l00836"></a><span class="lineno">  836</span>&#160;}</div><div class="line"><a name="l00837"></a><span class="lineno">  837</span>&#160;</div><div class="line"><a name="l00844"></a><span class="lineno">  844</span>&#160;<span class="keyword">template</span> &lt;<span class="keyword">typename</span> T&gt;</div><div class="line"><a name="l00845"></a><span class="lineno"><a class="line" href="../../d6/db1/class_floating_base_model.html#a52b52a8b4c0460bd3520fc4b5356e1f9">  845</a></span>&#160;<a class="code" href="../../d9/d8e/cpp_types_8h.html#ae2e5a0a178a2c528d8aa939f85db7a3e">DVec&lt;T&gt;</a> <a class="code" href="../../d6/db1/class_floating_base_model.html#a52b52a8b4c0460bd3520fc4b5356e1f9">FloatingBaseModel&lt;T&gt;::inverseDynamics</a>(</div><div class="line"><a name="l00846"></a><span class="lineno">  846</span>&#160;    <span class="keyword">const</span> <a class="code" href="../../de/dce/struct_f_b_model_state_derivative.html">FBModelStateDerivative&lt;T&gt;</a> &amp;dState) {</div><div class="line"><a name="l00847"></a><span class="lineno">  847</span>&#160;  setDState(dState);</div><div class="line"><a name="l00848"></a><span class="lineno">  848</span>&#160;  forwardAccelerationKinematics();</div><div class="line"><a name="l00849"></a><span class="lineno">  849</span>&#160;</div><div class="line"><a name="l00850"></a><span class="lineno">  850</span>&#160;  <span class="comment">// Spatial force for floating base</span></div><div class="line"><a name="l00851"></a><span class="lineno">  851</span>&#160;  <a class="code" href="../../d9/d8e/cpp_types_8h.html#a3d70f8eb9cf1855ca989f887a5543acc">SVec&lt;T&gt;</a> hb = _Ibody[5].getMatrix() * _v[5];</div><div class="line"><a name="l00852"></a><span class="lineno">  852</span>&#160;  _f[5] = _Ibody[5].getMatrix() * _a[5] + <a class="code" href="../../de/d1a/namespacespatial.html#a705e44e685fd39dbd6c1a9002ae9a344">forceCrossProduct</a>(_v[5], hb);</div><div class="line"><a name="l00853"></a><span class="lineno">  853</span>&#160;</div><div class="line"><a name="l00854"></a><span class="lineno">  854</span>&#160;  <span class="comment">// loop through joints</span></div><div class="line"><a name="l00855"></a><span class="lineno">  855</span>&#160;  <span class="keywordflow">for</span> (<span class="keywordtype">size_t</span> i = 6; i &lt; _nDof; i++) {</div><div class="line"><a name="l00856"></a><span class="lineno">  856</span>&#160;    <span class="comment">// spatial momentum</span></div><div class="line"><a name="l00857"></a><span class="lineno">  857</span>&#160;    <a class="code" href="../../d9/d8e/cpp_types_8h.html#a3d70f8eb9cf1855ca989f887a5543acc">SVec&lt;T&gt;</a> hi = _Ibody[i].getMatrix() * _v[i];</div><div class="line"><a name="l00858"></a><span class="lineno">  858</span>&#160;    <a class="code" href="../../d9/d8e/cpp_types_8h.html#a3d70f8eb9cf1855ca989f887a5543acc">SVec&lt;T&gt;</a> hr = _Irot[i].getMatrix() * _vrot[i];</div><div class="line"><a name="l00859"></a><span class="lineno">  859</span>&#160;</div><div class="line"><a name="l00860"></a><span class="lineno">  860</span>&#160;    <span class="comment">// spatial force</span></div><div class="line"><a name="l00861"></a><span class="lineno">  861</span>&#160;    _f[i] = _Ibody[i].getMatrix() * _a[i] + <a class="code" href="../../de/d1a/namespacespatial.html#a705e44e685fd39dbd6c1a9002ae9a344">forceCrossProduct</a>(_v[i], hi);</div><div class="line"><a name="l00862"></a><span class="lineno">  862</span>&#160;    _frot[i] =</div><div class="line"><a name="l00863"></a><span class="lineno">  863</span>&#160;        _Irot[i].getMatrix() * _arot[i] + <a class="code" href="../../de/d1a/namespacespatial.html#a705e44e685fd39dbd6c1a9002ae9a344">forceCrossProduct</a>(_vrot[i], hr);</div><div class="line"><a name="l00864"></a><span class="lineno">  864</span>&#160;  }</div><div class="line"><a name="l00865"></a><span class="lineno">  865</span>&#160;</div><div class="line"><a name="l00866"></a><span class="lineno">  866</span>&#160;  <a class="code" href="../../d9/d8e/cpp_types_8h.html#ae2e5a0a178a2c528d8aa939f85db7a3e">DVec&lt;T&gt;</a> genForce(_nDof);</div><div class="line"><a name="l00867"></a><span class="lineno">  867</span>&#160;  <span class="keywordflow">for</span> (<span class="keywordtype">size_t</span> i = _nDof - 1; i &gt; 5; i--) {</div><div class="line"><a name="l00868"></a><span class="lineno">  868</span>&#160;    <span class="comment">// Pull off compoents of force along the joint</span></div><div class="line"><a name="l00869"></a><span class="lineno">  869</span>&#160;    genForce[i] = _S[i].dot(_f[i]) + _Srot[i].dot(_frot[i]);</div><div class="line"><a name="l00870"></a><span class="lineno">  870</span>&#160;</div><div class="line"><a name="l00871"></a><span class="lineno">  871</span>&#160;    <span class="comment">// Propagate down the tree</span></div><div class="line"><a name="l00872"></a><span class="lineno">  872</span>&#160;    _f[_parents[i]] += _Xup[i].transpose() * _f[i];</div><div class="line"><a name="l00873"></a><span class="lineno">  873</span>&#160;    _f[_parents[i]] += _Xuprot[i].transpose() * _frot[i];</div><div class="line"><a name="l00874"></a><span class="lineno">  874</span>&#160;  }</div><div class="line"><a name="l00875"></a><span class="lineno">  875</span>&#160;  genForce.template head&lt;6&gt;() = _f[5];</div><div class="line"><a name="l00876"></a><span class="lineno">  876</span>&#160;  <span class="keywordflow">return</span> genForce;</div><div class="line"><a name="l00877"></a><span class="lineno">  877</span>&#160;}</div><div class="line"><a name="l00878"></a><span class="lineno">  878</span>&#160;</div><div class="line"><a name="l00879"></a><span class="lineno">  879</span>&#160;<span class="keyword">template</span> &lt;<span class="keyword">typename</span> T&gt;</div><div class="line"><a name="l00880"></a><span class="lineno"><a class="line" href="../../d6/db1/class_floating_base_model.html#a620e24c0c340885940f610875a35f3c7">  880</a></span>&#160;<span class="keywordtype">void</span> <a class="code" href="../../d6/db1/class_floating_base_model.html#a620e24c0c340885940f610875a35f3c7">FloatingBaseModel&lt;T&gt;::runABA</a>(<span class="keyword">const</span> <a class="code" href="../../d9/d8e/cpp_types_8h.html#ae2e5a0a178a2c528d8aa939f85db7a3e">DVec&lt;T&gt;</a> &amp;tau,</div><div class="line"><a name="l00881"></a><span class="lineno">  881</span>&#160;                                  <a class="code" href="../../de/dce/struct_f_b_model_state_derivative.html">FBModelStateDerivative&lt;T&gt;</a> &amp;dstate) {</div><div class="line"><a name="l00882"></a><span class="lineno">  882</span>&#160;  (void)tau;</div><div class="line"><a name="l00883"></a><span class="lineno">  883</span>&#160;  forwardKinematics();</div><div class="line"><a name="l00884"></a><span class="lineno">  884</span>&#160;  updateArticulatedBodies();</div><div class="line"><a name="l00885"></a><span class="lineno">  885</span>&#160;</div><div class="line"><a name="l00886"></a><span class="lineno">  886</span>&#160;  <span class="comment">// create spatial vector for gravity</span></div><div class="line"><a name="l00887"></a><span class="lineno">  887</span>&#160;  <a class="code" href="../../d9/d8e/cpp_types_8h.html#a3d70f8eb9cf1855ca989f887a5543acc">SVec&lt;T&gt;</a> aGravity;</div><div class="line"><a name="l00888"></a><span class="lineno">  888</span>&#160;  aGravity &lt;&lt; 0, 0, 0, _gravity[0], _gravity[1], _gravity[2];</div><div class="line"><a name="l00889"></a><span class="lineno">  889</span>&#160;</div><div class="line"><a name="l00890"></a><span class="lineno">  890</span>&#160;  <span class="comment">// float-base articulated inertia</span></div><div class="line"><a name="l00891"></a><span class="lineno">  891</span>&#160;  <a class="code" href="../../d9/d8e/cpp_types_8h.html#a3d70f8eb9cf1855ca989f887a5543acc">SVec&lt;T&gt;</a> ivProduct = _Ibody[5].getMatrix() * _v[5];</div><div class="line"><a name="l00892"></a><span class="lineno">  892</span>&#160;  _pA[5] = <a class="code" href="../../de/d1a/namespacespatial.html#a705e44e685fd39dbd6c1a9002ae9a344">forceCrossProduct</a>(_v[5], ivProduct);</div><div class="line"><a name="l00893"></a><span class="lineno">  893</span>&#160;</div><div class="line"><a name="l00894"></a><span class="lineno">  894</span>&#160;  <span class="comment">// loop 1, down the tree</span></div><div class="line"><a name="l00895"></a><span class="lineno">  895</span>&#160;  <span class="keywordflow">for</span> (<span class="keywordtype">size_t</span> i = 6; i &lt; _nDof; i++) {</div><div class="line"><a name="l00896"></a><span class="lineno">  896</span>&#160;    ivProduct = _Ibody[i].getMatrix() * _v[i];</div><div class="line"><a name="l00897"></a><span class="lineno">  897</span>&#160;    _pA[i] = <a class="code" href="../../de/d1a/namespacespatial.html#a705e44e685fd39dbd6c1a9002ae9a344">forceCrossProduct</a>(_v[i], ivProduct);</div><div class="line"><a name="l00898"></a><span class="lineno">  898</span>&#160;</div><div class="line"><a name="l00899"></a><span class="lineno">  899</span>&#160;    <span class="comment">// same for rotors</span></div><div class="line"><a name="l00900"></a><span class="lineno">  900</span>&#160;    <a class="code" href="../../d9/d8e/cpp_types_8h.html#a3d70f8eb9cf1855ca989f887a5543acc">SVec&lt;T&gt;</a> vJrot = _Srot[i] * _state.qd[i - 6];</div><div class="line"><a name="l00901"></a><span class="lineno">  901</span>&#160;    _vrot[i] = _Xuprot[i] * _v[_parents[i]] + vJrot;</div><div class="line"><a name="l00902"></a><span class="lineno">  902</span>&#160;    _crot[i] = <a class="code" href="../../de/d1a/namespacespatial.html#a49f73699d3c57d07835e8ab61af3f34a">motionCrossProduct</a>(_vrot[i], vJrot);</div><div class="line"><a name="l00903"></a><span class="lineno">  903</span>&#160;    ivProduct = _Irot[i].getMatrix() * _vrot[i];</div><div class="line"><a name="l00904"></a><span class="lineno">  904</span>&#160;    _pArot[i] = <a class="code" href="../../de/d1a/namespacespatial.html#a705e44e685fd39dbd6c1a9002ae9a344">forceCrossProduct</a>(_vrot[i], ivProduct);</div><div class="line"><a name="l00905"></a><span class="lineno">  905</span>&#160;  }</div><div class="line"><a name="l00906"></a><span class="lineno">  906</span>&#160;</div><div class="line"><a name="l00907"></a><span class="lineno">  907</span>&#160;  <span class="comment">// adjust pA for external forces</span></div><div class="line"><a name="l00908"></a><span class="lineno">  908</span>&#160;  <span class="keywordflow">for</span> (<span class="keywordtype">size_t</span> i = 5; i &lt; _nDof; i++) {</div><div class="line"><a name="l00909"></a><span class="lineno">  909</span>&#160;    <span class="comment">// TODO add if statement (avoid these calculations if the force is zero)</span></div><div class="line"><a name="l00910"></a><span class="lineno">  910</span>&#160;    <a class="code" href="../../d9/d8e/cpp_types_8h.html#aa8cb424e643d476ea485238dad7c3e18">Mat3&lt;T&gt;</a> R = <a class="code" href="../../de/d1a/namespacespatial.html#abdc3cd991206414d80d9f89be27e3633">rotationFromSXform</a>(_Xa[i]);</div><div class="line"><a name="l00911"></a><span class="lineno">  911</span>&#160;    <a class="code" href="../../d9/d8e/cpp_types_8h.html#a6f07728c866b15ff0676a9b2ebfd2ba6">Vec3&lt;T&gt;</a> p = <a class="code" href="../../de/d1a/namespacespatial.html#aba5e712bd3844d2e6a57dcb071f3f0a5">translationFromSXform</a>(_Xa[i]);</div><div class="line"><a name="l00912"></a><span class="lineno">  912</span>&#160;    <a class="code" href="../../d9/d8e/cpp_types_8h.html#ae8ab6bd4079a5317297be5eeb331b1ca">Mat6&lt;T&gt;</a> iX = <a class="code" href="../../de/d1a/namespacespatial.html#ae4a2145f3028087511b4f17c0152ec83">createSXform</a>(R.transpose(), -R * p);</div><div class="line"><a name="l00913"></a><span class="lineno">  913</span>&#160;    _pA[i] = _pA[i] - iX.transpose() * _externalForces.at(i);</div><div class="line"><a name="l00914"></a><span class="lineno">  914</span>&#160;  }</div><div class="line"><a name="l00915"></a><span class="lineno">  915</span>&#160;</div><div class="line"><a name="l00916"></a><span class="lineno">  916</span>&#160;  <span class="comment">// Pat&#39;s magic principle of least constraint</span></div><div class="line"><a name="l00917"></a><span class="lineno">  917</span>&#160;  <span class="keywordflow">for</span> (<span class="keywordtype">size_t</span> i = _nDof - 1; i &gt;= 6; i--) {</div><div class="line"><a name="l00918"></a><span class="lineno">  918</span>&#160;    _u[i] = tau[i - 6] - _S[i].transpose() * _pA[i] -</div><div class="line"><a name="l00919"></a><span class="lineno">  919</span>&#160;            _Srot[i].transpose() * _pArot[i] - _U[i].transpose() * _c[i] -</div><div class="line"><a name="l00920"></a><span class="lineno">  920</span>&#160;            _Urot[i].transpose() * _crot[i];</div><div class="line"><a name="l00921"></a><span class="lineno">  921</span>&#160;</div><div class="line"><a name="l00922"></a><span class="lineno">  922</span>&#160;    <span class="comment">// articulated inertia recursion</span></div><div class="line"><a name="l00923"></a><span class="lineno">  923</span>&#160;    <a class="code" href="../../d9/d8e/cpp_types_8h.html#a3d70f8eb9cf1855ca989f887a5543acc">SVec&lt;T&gt;</a> pa =</div><div class="line"><a name="l00924"></a><span class="lineno">  924</span>&#160;        _Xup[i].transpose() * (_pA[i] + _IA[i] * _c[i]) +</div><div class="line"><a name="l00925"></a><span class="lineno">  925</span>&#160;        _Xuprot[i].transpose() * (_pArot[i] + _Irot[i].getMatrix() * _crot[i]) +</div><div class="line"><a name="l00926"></a><span class="lineno">  926</span>&#160;        _Utot[i] * _u[i] / _d[i];</div><div class="line"><a name="l00927"></a><span class="lineno">  927</span>&#160;    _pA[_parents[i]] += pa;</div><div class="line"><a name="l00928"></a><span class="lineno">  928</span>&#160;  }</div><div class="line"><a name="l00929"></a><span class="lineno">  929</span>&#160;</div><div class="line"><a name="l00930"></a><span class="lineno">  930</span>&#160;  <span class="comment">// include gravity and compute acceleration of floating base</span></div><div class="line"><a name="l00931"></a><span class="lineno">  931</span>&#160;  <a class="code" href="../../d9/d8e/cpp_types_8h.html#a3d70f8eb9cf1855ca989f887a5543acc">SVec&lt;T&gt;</a> a0 = -aGravity;</div><div class="line"><a name="l00932"></a><span class="lineno">  932</span>&#160;  <a class="code" href="../../d9/d8e/cpp_types_8h.html#a3d70f8eb9cf1855ca989f887a5543acc">SVec&lt;T&gt;</a> ub = -_pA[5];</div><div class="line"><a name="l00933"></a><span class="lineno">  933</span>&#160;  _a[5] = _Xup[5] * a0;</div><div class="line"><a name="l00934"></a><span class="lineno">  934</span>&#160;  <a class="code" href="../../d9/d8e/cpp_types_8h.html#a3d70f8eb9cf1855ca989f887a5543acc">SVec&lt;T&gt;</a> afb = _invIA5.solve(ub - _IA[5].transpose() * _a[5]);</div><div class="line"><a name="l00935"></a><span class="lineno">  935</span>&#160;  _a[5] += afb;</div><div class="line"><a name="l00936"></a><span class="lineno">  936</span>&#160;</div><div class="line"><a name="l00937"></a><span class="lineno">  937</span>&#160;  <span class="comment">// joint accelerations</span></div><div class="line"><a name="l00938"></a><span class="lineno">  938</span>&#160;  dstate.<a class="code" href="../../de/dce/struct_f_b_model_state_derivative.html#af32a2041b72e6b30ed1c8a027d340fd2">qdd</a> = <a class="code" href="../../d9/d8e/cpp_types_8h.html#ae2e5a0a178a2c528d8aa939f85db7a3e">DVec&lt;T&gt;</a>(_nDof - 6);</div><div class="line"><a name="l00939"></a><span class="lineno">  939</span>&#160;  <span class="keywordflow">for</span> (<span class="keywordtype">size_t</span> i = 6; i &lt; _nDof; i++) {</div><div class="line"><a name="l00940"></a><span class="lineno">  940</span>&#160;    dstate.<a class="code" href="../../de/dce/struct_f_b_model_state_derivative.html#af32a2041b72e6b30ed1c8a027d340fd2">qdd</a>[i - 6] =</div><div class="line"><a name="l00941"></a><span class="lineno">  941</span>&#160;        (_u[i] - _Utot[i].transpose() * _a[_parents[i]]) / _d[i];</div><div class="line"><a name="l00942"></a><span class="lineno">  942</span>&#160;    _a[i] = _Xup[i] * _a[_parents[i]] + _S[i] * dstate.<a class="code" href="../../de/dce/struct_f_b_model_state_derivative.html#af32a2041b72e6b30ed1c8a027d340fd2">qdd</a>[i - 6] + _c[i];</div><div class="line"><a name="l00943"></a><span class="lineno">  943</span>&#160;  }</div><div class="line"><a name="l00944"></a><span class="lineno">  944</span>&#160;</div><div class="line"><a name="l00945"></a><span class="lineno">  945</span>&#160;  <span class="comment">// output</span></div><div class="line"><a name="l00946"></a><span class="lineno">  946</span>&#160;  <a class="code" href="../../d9/d8e/cpp_types_8h.html#a0b83d93a3c74d282558436e26c056896">RotMat&lt;T&gt;</a> Rup = <a class="code" href="../../de/d1a/namespacespatial.html#abdc3cd991206414d80d9f89be27e3633">rotationFromSXform</a>(_Xup[5]);</div><div class="line"><a name="l00947"></a><span class="lineno">  947</span>&#160;  dstate.<a class="code" href="../../de/dce/struct_f_b_model_state_derivative.html#a825cfc02adde2c48b0d508a3bf77e896">dBodyPosition</a> =</div><div class="line"><a name="l00948"></a><span class="lineno">  948</span>&#160;      Rup.transpose() * _state.bodyVelocity.template block&lt;3, 1&gt;(3, 0);</div><div class="line"><a name="l00949"></a><span class="lineno">  949</span>&#160;  dstate.<a class="code" href="../../de/dce/struct_f_b_model_state_derivative.html#a8b7b6d70f1b1268f02e1b291dad26c5d">dBodyVelocity</a> = afb;</div><div class="line"><a name="l00950"></a><span class="lineno">  950</span>&#160;  <span class="comment">// qdd is set in the for loop above</span></div><div class="line"><a name="l00951"></a><span class="lineno">  951</span>&#160;}</div><div class="line"><a name="l00952"></a><span class="lineno">  952</span>&#160;</div><div class="line"><a name="l00961"></a><span class="lineno">  961</span>&#160;<span class="keyword">template</span> &lt;<span class="keyword">typename</span> T&gt;</div><div class="line"><a name="l00962"></a><span class="lineno"><a class="line" href="../../d6/db1/class_floating_base_model.html#a1edbea0371cffe8692a83ae5a9396e41">  962</a></span>&#160;T <a class="code" href="../../d6/db1/class_floating_base_model.html#a1edbea0371cffe8692a83ae5a9396e41">FloatingBaseModel&lt;T&gt;::applyTestForce</a>(<span class="keyword">const</span> <span class="keywordtype">int</span> gc_index,</div><div class="line"><a name="l00963"></a><span class="lineno">  963</span>&#160;                                       <span class="keyword">const</span> <a class="code" href="../../d9/d8e/cpp_types_8h.html#a6f07728c866b15ff0676a9b2ebfd2ba6">Vec3&lt;T&gt;</a> &amp;force_ics_at_contact,</div><div class="line"><a name="l00964"></a><span class="lineno">  964</span>&#160;                                       <a class="code" href="../../de/dce/struct_f_b_model_state_derivative.html">FBModelStateDerivative&lt;T&gt;</a> &amp;dstate_out) {</div><div class="line"><a name="l00965"></a><span class="lineno">  965</span>&#160;  forwardKinematics();</div><div class="line"><a name="l00966"></a><span class="lineno">  966</span>&#160;  updateArticulatedBodies();</div><div class="line"><a name="l00967"></a><span class="lineno">  967</span>&#160;  updateForcePropagators();</div><div class="line"><a name="l00968"></a><span class="lineno">  968</span>&#160;  udpateQddEffects();</div><div class="line"><a name="l00969"></a><span class="lineno">  969</span>&#160;</div><div class="line"><a name="l00970"></a><span class="lineno">  970</span>&#160;  <span class="keywordtype">size_t</span> i_opsp = _gcParent.at(gc_index);</div><div class="line"><a name="l00971"></a><span class="lineno">  971</span>&#160;  <span class="keywordtype">size_t</span> i = i_opsp;</div><div class="line"><a name="l00972"></a><span class="lineno">  972</span>&#160;</div><div class="line"><a name="l00973"></a><span class="lineno">  973</span>&#160;  dstate_out.<a class="code" href="../../de/dce/struct_f_b_model_state_derivative.html#af32a2041b72e6b30ed1c8a027d340fd2">qdd</a>.setZero();</div><div class="line"><a name="l00974"></a><span class="lineno">  974</span>&#160;</div><div class="line"><a name="l00975"></a><span class="lineno">  975</span>&#160;  <span class="comment">// Rotation to absolute coords</span></div><div class="line"><a name="l00976"></a><span class="lineno">  976</span>&#160;  <a class="code" href="../../d9/d8e/cpp_types_8h.html#aa8cb424e643d476ea485238dad7c3e18">Mat3&lt;T&gt;</a> Rai = _Xa[i].template block&lt;3, 3&gt;(0, 0).transpose();</div><div class="line"><a name="l00977"></a><span class="lineno">  977</span>&#160;  <a class="code" href="../../d9/d8e/cpp_types_8h.html#ae8ab6bd4079a5317297be5eeb331b1ca">Mat6&lt;T&gt;</a> Xc = <a class="code" href="../../de/d1a/namespacespatial.html#ae4a2145f3028087511b4f17c0152ec83">createSXform</a>(Rai, _gcLocation.at(gc_index));</div><div class="line"><a name="l00978"></a><span class="lineno">  978</span>&#160;</div><div class="line"><a name="l00979"></a><span class="lineno">  979</span>&#160;  <span class="comment">// D is one column of an extended force propagator matrix (See Wensing, 2012</span></div><div class="line"><a name="l00980"></a><span class="lineno">  980</span>&#160;  <span class="comment">// ICRA)</span></div><div class="line"><a name="l00981"></a><span class="lineno">  981</span>&#160;  <a class="code" href="../../d9/d8e/cpp_types_8h.html#a3d70f8eb9cf1855ca989f887a5543acc">SVec&lt;T&gt;</a> F = Xc.transpose().template rightCols&lt;3&gt;() * force_ics_at_contact;</div><div class="line"><a name="l00982"></a><span class="lineno">  982</span>&#160;</div><div class="line"><a name="l00983"></a><span class="lineno">  983</span>&#160;  <span class="keywordtype">double</span> LambdaInv = 0;</div><div class="line"><a name="l00984"></a><span class="lineno">  984</span>&#160;  <span class="keywordtype">double</span> tmp = 0;</div><div class="line"><a name="l00985"></a><span class="lineno">  985</span>&#160;</div><div class="line"><a name="l00986"></a><span class="lineno">  986</span>&#160;  <span class="comment">// from tips to base</span></div><div class="line"><a name="l00987"></a><span class="lineno">  987</span>&#160;  <span class="keywordflow">while</span> (i &gt; 5) {</div><div class="line"><a name="l00988"></a><span class="lineno">  988</span>&#160;    tmp = F.dot(_S[i]);</div><div class="line"><a name="l00989"></a><span class="lineno">  989</span>&#160;    LambdaInv += tmp * tmp / _d[i];</div><div class="line"><a name="l00990"></a><span class="lineno">  990</span>&#160;    dstate_out.<a class="code" href="../../de/dce/struct_f_b_model_state_derivative.html#af32a2041b72e6b30ed1c8a027d340fd2">qdd</a> += _qdd_from_subqdd.col(i - 6) * tmp / _d[i];</div><div class="line"><a name="l00991"></a><span class="lineno">  991</span>&#160;</div><div class="line"><a name="l00992"></a><span class="lineno">  992</span>&#160;    <span class="comment">// Apply force propagator (see Pat&#39;s ICRA 2012 paper)</span></div><div class="line"><a name="l00993"></a><span class="lineno">  993</span>&#160;    <span class="comment">// essentially, since the joint is articulated, only a portion of the force</span></div><div class="line"><a name="l00994"></a><span class="lineno">  994</span>&#160;    <span class="comment">// is felt on the predecessor. So, while Xup^T sends a force backwards as if</span></div><div class="line"><a name="l00995"></a><span class="lineno">  995</span>&#160;    <span class="comment">// the joint was locked, ChiUp^T sends the force backward as if the joint</span></div><div class="line"><a name="l00996"></a><span class="lineno">  996</span>&#160;    <span class="comment">// were free</span></div><div class="line"><a name="l00997"></a><span class="lineno">  997</span>&#160;    F = _ChiUp[i].transpose() * F;</div><div class="line"><a name="l00998"></a><span class="lineno">  998</span>&#160;    i = _parents[i];</div><div class="line"><a name="l00999"></a><span class="lineno">  999</span>&#160;  }</div><div class="line"><a name="l01000"></a><span class="lineno"> 1000</span>&#160;</div><div class="line"><a name="l01001"></a><span class="lineno"> 1001</span>&#160;  <span class="comment">// TODO: Only carry out the QR once within update Aritculated Bodies</span></div><div class="line"><a name="l01002"></a><span class="lineno"> 1002</span>&#160;  dstate_out.<a class="code" href="../../de/dce/struct_f_b_model_state_derivative.html#a8b7b6d70f1b1268f02e1b291dad26c5d">dBodyVelocity</a> = _invIA5.solve(F);</div><div class="line"><a name="l01003"></a><span class="lineno"> 1003</span>&#160;  LambdaInv += F.dot(dstate_out.<a class="code" href="../../de/dce/struct_f_b_model_state_derivative.html#a8b7b6d70f1b1268f02e1b291dad26c5d">dBodyVelocity</a>);</div><div class="line"><a name="l01004"></a><span class="lineno"> 1004</span>&#160;  dstate_out.<a class="code" href="../../de/dce/struct_f_b_model_state_derivative.html#af32a2041b72e6b30ed1c8a027d340fd2">qdd</a> += _qdd_from_base_accel * dstate_out.<a class="code" href="../../de/dce/struct_f_b_model_state_derivative.html#a8b7b6d70f1b1268f02e1b291dad26c5d">dBodyVelocity</a>;</div><div class="line"><a name="l01005"></a><span class="lineno"> 1005</span>&#160;</div><div class="line"><a name="l01006"></a><span class="lineno"> 1006</span>&#160;  <span class="keywordflow">return</span> LambdaInv;</div><div class="line"><a name="l01007"></a><span class="lineno"> 1007</span>&#160;}</div><div class="line"><a name="l01008"></a><span class="lineno"> 1008</span>&#160;</div><div class="line"><a name="l01016"></a><span class="lineno"> 1016</span>&#160;<span class="keyword">template</span> &lt;<span class="keyword">typename</span> T&gt;</div><div class="line"><a name="l01017"></a><span class="lineno"><a class="line" href="../../d6/db1/class_floating_base_model.html#a423baeb491e9bc75a91ddde370c3f647"> 1017</a></span>&#160;T <a class="code" href="../../d6/db1/class_floating_base_model.html#a759338421e6adbe3a0a69e42271d43d4">FloatingBaseModel&lt;T&gt;::invContactInertia</a>(<span class="keyword">const</span> <span class="keywordtype">int</span> gc_index,</div><div class="line"><a name="l01018"></a><span class="lineno"> 1018</span>&#160;                                          <span class="keyword">const</span> <a class="code" href="../../d9/d8e/cpp_types_8h.html#a6f07728c866b15ff0676a9b2ebfd2ba6">Vec3&lt;T&gt;</a> &amp;force_ics_at_contact) {</div><div class="line"><a name="l01019"></a><span class="lineno"> 1019</span>&#160;  forwardKinematics();</div><div class="line"><a name="l01020"></a><span class="lineno"> 1020</span>&#160;  updateArticulatedBodies();</div><div class="line"><a name="l01021"></a><span class="lineno"> 1021</span>&#160;  updateForcePropagators();</div><div class="line"><a name="l01022"></a><span class="lineno"> 1022</span>&#160;</div><div class="line"><a name="l01023"></a><span class="lineno"> 1023</span>&#160;  <span class="keywordtype">size_t</span> i_opsp = _gcParent.at(gc_index);</div><div class="line"><a name="l01024"></a><span class="lineno"> 1024</span>&#160;  <span class="keywordtype">size_t</span> i = i_opsp;</div><div class="line"><a name="l01025"></a><span class="lineno"> 1025</span>&#160;</div><div class="line"><a name="l01026"></a><span class="lineno"> 1026</span>&#160;  <span class="comment">// Rotation to absolute coords</span></div><div class="line"><a name="l01027"></a><span class="lineno"> 1027</span>&#160;  <a class="code" href="../../d9/d8e/cpp_types_8h.html#aa8cb424e643d476ea485238dad7c3e18">Mat3&lt;T&gt;</a> Rai = _Xa[i].template block&lt;3, 3&gt;(0, 0).transpose();</div><div class="line"><a name="l01028"></a><span class="lineno"> 1028</span>&#160;  <a class="code" href="../../d9/d8e/cpp_types_8h.html#ae8ab6bd4079a5317297be5eeb331b1ca">Mat6&lt;T&gt;</a> Xc = <a class="code" href="../../de/d1a/namespacespatial.html#ae4a2145f3028087511b4f17c0152ec83">createSXform</a>(Rai, _gcLocation.at(gc_index));</div><div class="line"><a name="l01029"></a><span class="lineno"> 1029</span>&#160;</div><div class="line"><a name="l01030"></a><span class="lineno"> 1030</span>&#160;  <span class="comment">// D is one column of an extended force propagator matrix (See Wensing, 2012</span></div><div class="line"><a name="l01031"></a><span class="lineno"> 1031</span>&#160;  <span class="comment">// ICRA)</span></div><div class="line"><a name="l01032"></a><span class="lineno"> 1032</span>&#160;  <a class="code" href="../../d9/d8e/cpp_types_8h.html#a3d70f8eb9cf1855ca989f887a5543acc">SVec&lt;T&gt;</a> F = Xc.transpose().template rightCols&lt;3&gt;() * force_ics_at_contact;</div><div class="line"><a name="l01033"></a><span class="lineno"> 1033</span>&#160;</div><div class="line"><a name="l01034"></a><span class="lineno"> 1034</span>&#160;  <span class="keywordtype">double</span> LambdaInv = 0;</div><div class="line"><a name="l01035"></a><span class="lineno"> 1035</span>&#160;  <span class="keywordtype">double</span> tmp = 0;</div><div class="line"><a name="l01036"></a><span class="lineno"> 1036</span>&#160;</div><div class="line"><a name="l01037"></a><span class="lineno"> 1037</span>&#160;  <span class="comment">// from tips to base</span></div><div class="line"><a name="l01038"></a><span class="lineno"> 1038</span>&#160;  <span class="keywordflow">while</span> (i &gt; 5) {</div><div class="line"><a name="l01039"></a><span class="lineno"> 1039</span>&#160;    tmp = F.dot(_S[i]);</div><div class="line"><a name="l01040"></a><span class="lineno"> 1040</span>&#160;    LambdaInv += tmp * tmp / _d[i];</div><div class="line"><a name="l01041"></a><span class="lineno"> 1041</span>&#160;</div><div class="line"><a name="l01042"></a><span class="lineno"> 1042</span>&#160;    <span class="comment">// Apply force propagator (see Pat&#39;s ICRA 2012 paper)</span></div><div class="line"><a name="l01043"></a><span class="lineno"> 1043</span>&#160;    <span class="comment">// essentially, since the joint is articulated, only a portion of the force</span></div><div class="line"><a name="l01044"></a><span class="lineno"> 1044</span>&#160;    <span class="comment">// is felt on the predecessor. So, while Xup^T sends a force backwards as if</span></div><div class="line"><a name="l01045"></a><span class="lineno"> 1045</span>&#160;    <span class="comment">// the joint was locked, ChiUp^T sends the force backward as if the joint</span></div><div class="line"><a name="l01046"></a><span class="lineno"> 1046</span>&#160;    <span class="comment">// were free</span></div><div class="line"><a name="l01047"></a><span class="lineno"> 1047</span>&#160;    F = _ChiUp[i].transpose() * F;</div><div class="line"><a name="l01048"></a><span class="lineno"> 1048</span>&#160;    i = _parents[i];</div><div class="line"><a name="l01049"></a><span class="lineno"> 1049</span>&#160;  }</div><div class="line"><a name="l01050"></a><span class="lineno"> 1050</span>&#160;  LambdaInv += F.dot(_invIA5.solve(F));</div><div class="line"><a name="l01051"></a><span class="lineno"> 1051</span>&#160;  <span class="keywordflow">return</span> LambdaInv;</div><div class="line"><a name="l01052"></a><span class="lineno"> 1052</span>&#160;}</div><div class="line"><a name="l01053"></a><span class="lineno"> 1053</span>&#160;</div><div class="line"><a name="l01064"></a><span class="lineno"> 1064</span>&#160;<span class="keyword">template</span> &lt;<span class="keyword">typename</span> T&gt;</div><div class="line"><a name="l01065"></a><span class="lineno"><a class="line" href="../../d6/db1/class_floating_base_model.html#a759338421e6adbe3a0a69e42271d43d4"> 1065</a></span>&#160;<a class="code" href="../../d9/d8e/cpp_types_8h.html#a807200f3771dbd2cb2e4be52c5f2518c">DMat&lt;T&gt;</a> <a class="code" href="../../d6/db1/class_floating_base_model.html#a759338421e6adbe3a0a69e42271d43d4">FloatingBaseModel&lt;T&gt;::invContactInertia</a>(</div><div class="line"><a name="l01066"></a><span class="lineno"> 1066</span>&#160;    <span class="keyword">const</span> <span class="keywordtype">int</span> gc_index, <span class="keyword">const</span> <a class="code" href="../../d9/d8e/cpp_types_8h.html#a0231caceb234a1eace8c03dda00431dd">D6Mat&lt;T&gt;</a> &amp;force_directions) {</div><div class="line"><a name="l01067"></a><span class="lineno"> 1067</span>&#160;  forwardKinematics();</div><div class="line"><a name="l01068"></a><span class="lineno"> 1068</span>&#160;  updateArticulatedBodies();</div><div class="line"><a name="l01069"></a><span class="lineno"> 1069</span>&#160;  updateForcePropagators();</div><div class="line"><a name="l01070"></a><span class="lineno"> 1070</span>&#160;</div><div class="line"><a name="l01071"></a><span class="lineno"> 1071</span>&#160;  <span class="keywordtype">size_t</span> i_opsp = _gcParent.at(gc_index);</div><div class="line"><a name="l01072"></a><span class="lineno"> 1072</span>&#160;  <span class="keywordtype">size_t</span> i = i_opsp;</div><div class="line"><a name="l01073"></a><span class="lineno"> 1073</span>&#160;</div><div class="line"><a name="l01074"></a><span class="lineno"> 1074</span>&#160;  <span class="comment">// Rotation to absolute coords</span></div><div class="line"><a name="l01075"></a><span class="lineno"> 1075</span>&#160;  <a class="code" href="../../d9/d8e/cpp_types_8h.html#aa8cb424e643d476ea485238dad7c3e18">Mat3&lt;T&gt;</a> Rai = _Xa[i].template block&lt;3, 3&gt;(0, 0).transpose();</div><div class="line"><a name="l01076"></a><span class="lineno"> 1076</span>&#160;  <a class="code" href="../../d9/d8e/cpp_types_8h.html#ae8ab6bd4079a5317297be5eeb331b1ca">Mat6&lt;T&gt;</a> Xc = <a class="code" href="../../de/d1a/namespacespatial.html#ae4a2145f3028087511b4f17c0152ec83">createSXform</a>(Rai, _gcLocation.at(gc_index));</div><div class="line"><a name="l01077"></a><span class="lineno"> 1077</span>&#160;</div><div class="line"><a name="l01078"></a><span class="lineno"> 1078</span>&#160;  <span class="comment">// D is a subslice of an extended force propagator matrix (See Wensing, 2012</span></div><div class="line"><a name="l01079"></a><span class="lineno"> 1079</span>&#160;  <span class="comment">// ICRA)</span></div><div class="line"><a name="l01080"></a><span class="lineno"> 1080</span>&#160;  <a class="code" href="../../d9/d8e/cpp_types_8h.html#a0231caceb234a1eace8c03dda00431dd">D6Mat&lt;T&gt;</a> D = Xc.transpose() * force_directions;</div><div class="line"><a name="l01081"></a><span class="lineno"> 1081</span>&#160;</div><div class="line"><a name="l01082"></a><span class="lineno"> 1082</span>&#160;  <span class="keywordtype">size_t</span> m = force_directions.cols();</div><div class="line"><a name="l01083"></a><span class="lineno"> 1083</span>&#160;</div><div class="line"><a name="l01084"></a><span class="lineno"> 1084</span>&#160;  <a class="code" href="../../d9/d8e/cpp_types_8h.html#a807200f3771dbd2cb2e4be52c5f2518c">DMat&lt;T&gt;</a> LambdaInv = <a class="code" href="../../d9/d8e/cpp_types_8h.html#a807200f3771dbd2cb2e4be52c5f2518c">DMat&lt;T&gt;::Zero</a>(m, m);</div><div class="line"><a name="l01085"></a><span class="lineno"> 1085</span>&#160;  <a class="code" href="../../d9/d8e/cpp_types_8h.html#ae2e5a0a178a2c528d8aa939f85db7a3e">DVec&lt;T&gt;</a> tmp = <a class="code" href="../../d9/d8e/cpp_types_8h.html#ae2e5a0a178a2c528d8aa939f85db7a3e">DVec&lt;T&gt;::Zero</a>(m);</div><div class="line"><a name="l01086"></a><span class="lineno"> 1086</span>&#160;</div><div class="line"><a name="l01087"></a><span class="lineno"> 1087</span>&#160;  <span class="comment">// from tips to base</span></div><div class="line"><a name="l01088"></a><span class="lineno"> 1088</span>&#160;  <span class="keywordflow">while</span> (i &gt; 5) {</div><div class="line"><a name="l01089"></a><span class="lineno"> 1089</span>&#160;    tmp = D.transpose() * _S[i];</div><div class="line"><a name="l01090"></a><span class="lineno"> 1090</span>&#160;    LambdaInv += tmp * tmp.transpose() / _d[i];</div><div class="line"><a name="l01091"></a><span class="lineno"> 1091</span>&#160;</div><div class="line"><a name="l01092"></a><span class="lineno"> 1092</span>&#160;    <span class="comment">// Apply force propagator (see Pat&#39;s ICRA 2012 paper)</span></div><div class="line"><a name="l01093"></a><span class="lineno"> 1093</span>&#160;    <span class="comment">// essentially, since the joint is articulated, only a portion of the force</span></div><div class="line"><a name="l01094"></a><span class="lineno"> 1094</span>&#160;    <span class="comment">// is felt on the predecessor. So, while Xup^T sends a force backwards as if</span></div><div class="line"><a name="l01095"></a><span class="lineno"> 1095</span>&#160;    <span class="comment">// the joint was locked, ChiUp^T sends the force backward as if the joint</span></div><div class="line"><a name="l01096"></a><span class="lineno"> 1096</span>&#160;    <span class="comment">// were free</span></div><div class="line"><a name="l01097"></a><span class="lineno"> 1097</span>&#160;    D = _ChiUp[i].transpose() * D;</div><div class="line"><a name="l01098"></a><span class="lineno"> 1098</span>&#160;    i = _parents[i];</div><div class="line"><a name="l01099"></a><span class="lineno"> 1099</span>&#160;  }</div><div class="line"><a name="l01100"></a><span class="lineno"> 1100</span>&#160;</div><div class="line"><a name="l01101"></a><span class="lineno"> 1101</span>&#160;  <span class="comment">// TODO: Only carry out the QR once within update Aritculated Bodies</span></div><div class="line"><a name="l01102"></a><span class="lineno"> 1102</span>&#160;  LambdaInv += D.transpose() * _invIA5.solve(D);</div><div class="line"><a name="l01103"></a><span class="lineno"> 1103</span>&#160;</div><div class="line"><a name="l01104"></a><span class="lineno"> 1104</span>&#160;  <span class="keywordflow">return</span> LambdaInv;</div><div class="line"><a name="l01105"></a><span class="lineno"> 1105</span>&#160;}</div><div class="line"><a name="l01106"></a><span class="lineno"> 1106</span>&#160;</div><div class="line"><a name="l01107"></a><span class="lineno"> 1107</span>&#160;<span class="keyword">template</span> <span class="keyword">class </span><a class="code" href="../../d6/db1/class_floating_base_model.html">FloatingBaseModel&lt;double&gt;</a>;</div><div class="line"><a name="l01108"></a><span class="lineno"> 1108</span>&#160;<span class="keyword">template</span> <span class="keyword">class </span><a class="code" href="../../d6/db1/class_floating_base_model.html">FloatingBaseModel&lt;float&gt;</a>;</div><div class="ttc" id="class_floating_base_model_html_a52b52a8b4c0460bd3520fc4b5356e1f9"><div class="ttname"><a href="../../d6/db1/class_floating_base_model.html#a52b52a8b4c0460bd3520fc4b5356e1f9">FloatingBaseModel::inverseDynamics</a></div><div class="ttdeci">DVec&lt; T &gt; inverseDynamics(const FBModelStateDerivative&lt; T &gt; &amp;dState)</div><div class="ttdef"><b>Definition:</b> <a href="../../da/de7/_floating_base_model_8cpp_source.html#l00845">FloatingBaseModel.cpp:845</a></div></div>
<div class="ttc" id="class_floating_base_model_html_a72dd957e465044ebd8f007385bdf6b29"><div class="ttname"><a href="../../d6/db1/class_floating_base_model.html#a72dd957e465044ebd8f007385bdf6b29">FloatingBaseModel::getOrientation</a></div><div class="ttdeci">Mat3&lt; T &gt; getOrientation(const int link_idx)</div><div class="ttdef"><b>Definition:</b> <a href="../../da/de7/_floating_base_model_8cpp_source.html#l00671">FloatingBaseModel.cpp:671</a></div></div>
<div class="ttc" id="class_floating_base_model_html_a409343ac03c6aebaffdb288c6c65f892"><div class="ttname"><a href="../../d6/db1/class_floating_base_model.html#a409343ac03c6aebaffdb288c6c65f892">FloatingBaseModel::updateForcePropagators</a></div><div class="ttdeci">void updateForcePropagators()</div><div class="ttdef"><b>Definition:</b> <a href="../../da/de7/_floating_base_model_8cpp_source.html#l00129">FloatingBaseModel.cpp:129</a></div></div>
<div class="ttc" id="namespacespatial_html_a95b2db654a989f56df5a8977a5be12d6"><div class="ttname"><a href="../../de/d1a/namespacespatial.html#a95b2db654a989f56df5a8977a5be12d6">spatial::spatialToLinearVelocity</a></div><div class="ttdeci">auto spatialToLinearVelocity(const Eigen::MatrixBase&lt; T &gt; &amp;v, const Eigen::MatrixBase&lt; T2 &gt; &amp;x)</div><div class="ttdef"><b>Definition:</b> <a href="../../d9/dc8/spatial_8h_source.html#l00261">spatial.h:261</a></div></div>
<div class="ttc" id="class_floating_base_model_html_a620e24c0c340885940f610875a35f3c7"><div class="ttname"><a href="../../d6/db1/class_floating_base_model.html#a620e24c0c340885940f610875a35f3c7">FloatingBaseModel::runABA</a></div><div class="ttdeci">void runABA(const DVec&lt; T &gt; &amp;tau, FBModelStateDerivative&lt; T &gt; &amp;dstate)</div><div class="ttdef"><b>Definition:</b> <a href="../../da/de7/_floating_base_model_8cpp_source.html#l00880">FloatingBaseModel.cpp:880</a></div></div>
<div class="ttc" id="class_floating_base_model_html_a33ff105654083ed8c395097fd208b174"><div class="ttname"><a href="../../d6/db1/class_floating_base_model.html#a33ff105654083ed8c395097fd208b174">FloatingBaseModel::getAngularVelocity</a></div><div class="ttdeci">Vec3&lt; T &gt; getAngularVelocity(const int link_idx)</div><div class="ttdef"><b>Definition:</b> <a href="../../da/de7/_floating_base_model_8cpp_source.html#l00731">FloatingBaseModel.cpp:731</a></div></div>
<div class="ttc" id="namespacespatial_html_a705e44e685fd39dbd6c1a9002ae9a344"><div class="ttname"><a href="../../de/d1a/namespacespatial.html#a705e44e685fd39dbd6c1a9002ae9a344">spatial::forceCrossProduct</a></div><div class="ttdeci">auto forceCrossProduct(const Eigen::MatrixBase&lt; T &gt; &amp;a, const Eigen::MatrixBase&lt; T &gt; &amp;b)</div><div class="ttdef"><b>Definition:</b> <a href="../../d9/dc8/spatial_8h_source.html#l00091">spatial.h:91</a></div></div>
<div class="ttc" id="cpp_types_8h_html_a0231caceb234a1eace8c03dda00431dd"><div class="ttname"><a href="../../d9/d8e/cpp_types_8h.html#a0231caceb234a1eace8c03dda00431dd">D6Mat</a></div><div class="ttdeci">typename Eigen::Matrix&lt; T, 6, Eigen::Dynamic &gt; D6Mat</div><div class="ttdef"><b>Definition:</b> <a href="../../d9/d8e/cpp_types_8h_source.html#l00110">cppTypes.h:110</a></div></div>
<div class="ttc" id="cpp_types_8h_html_ae8ab6bd4079a5317297be5eeb331b1ca"><div class="ttname"><a href="../../d9/d8e/cpp_types_8h.html#ae8ab6bd4079a5317297be5eeb331b1ca">Mat6</a></div><div class="ttdeci">typename Eigen::Matrix&lt; T, 6, 6 &gt; Mat6</div><div class="ttdef"><b>Definition:</b> <a href="../../d9/d8e/cpp_types_8h_source.html#l00070">cppTypes.h:70</a></div></div>
<div class="ttc" id="class_floating_base_model_html_af6a72105c1c1ad7565d8dbbfaba401eb"><div class="ttname"><a href="../../d6/db1/class_floating_base_model.html#af6a72105c1c1ad7565d8dbbfaba401eb">FloatingBaseModel::check</a></div><div class="ttdeci">void check()</div><div class="ttdef"><b>Definition:</b> <a href="../../da/de7/_floating_base_model_8cpp_source.html#l00442">FloatingBaseModel.cpp:442</a></div></div>
<div class="ttc" id="namespacespatial_html_a6440807f3b1854b1708f49048331811a"><div class="ttname"><a href="../../de/d1a/namespacespatial.html#a6440807f3b1854b1708f49048331811a">spatial::jointXform</a></div><div class="ttdeci">Mat6&lt; T &gt; jointXform(JointType joint, CoordinateAxis axis, T q)</div><div class="ttdef"><b>Definition:</b> <a href="../../d9/dc8/spatial_8h_source.html#l00219">spatial.h:219</a></div></div>
<div class="ttc" id="struct_f_b_model_state_derivative_html_a825cfc02adde2c48b0d508a3bf77e896"><div class="ttname"><a href="../../de/dce/struct_f_b_model_state_derivative.html#a825cfc02adde2c48b0d508a3bf77e896">FBModelStateDerivative::dBodyPosition</a></div><div class="ttdeci">EIGEN_MAKE_ALIGNED_OPERATOR_NEW Vec3&lt; T &gt; dBodyPosition</div><div class="ttdef"><b>Definition:</b> <a href="../../d5/d9a/_floating_base_model_8h_source.html#l00059">FloatingBaseModel.h:59</a></div></div>
<div class="ttc" id="cpp_types_8h_html_aa8cb424e643d476ea485238dad7c3e18"><div class="ttname"><a href="../../d9/d8e/cpp_types_8h.html#aa8cb424e643d476ea485238dad7c3e18">Mat3</a></div><div class="ttdeci">typename Eigen::Matrix&lt; T, 3, 3 &gt; Mat3</div><div class="ttdef"><b>Definition:</b> <a href="../../d9/d8e/cpp_types_8h_source.html#l00054">cppTypes.h:54</a></div></div>
<div class="ttc" id="class_floating_base_model_html_a48ee0323aedafce849ca9dec1ed6d9c0"><div class="ttname"><a href="../../d6/db1/class_floating_base_model.html#a48ee0323aedafce849ca9dec1ed6d9c0">FloatingBaseModel::generalizedCoriolisForce</a></div><div class="ttdeci">DVec&lt; T &gt; generalizedCoriolisForce()</div><div class="ttdef"><b>Definition:</b> <a href="../../da/de7/_floating_base_model_8cpp_source.html#l00636">FloatingBaseModel.cpp:636</a></div></div>
<div class="ttc" id="class_floating_base_model_html_a3f2a319bbd26e5e7f91c7d3b980e28c4"><div class="ttname"><a href="../../d6/db1/class_floating_base_model.html#a3f2a319bbd26e5e7f91c7d3b980e28c4">FloatingBaseModel::addGroundContactPoint</a></div><div class="ttdeci">int addGroundContactPoint(int bodyID, const Vec3&lt; T &gt; &amp;location, bool isFoot=false)</div><div class="ttdef"><b>Definition:</b> <a href="../../da/de7/_floating_base_model_8cpp_source.html#l00319">FloatingBaseModel.cpp:319</a></div></div>
<div class="ttc" id="cpp_types_8h_html_a807200f3771dbd2cb2e4be52c5f2518c"><div class="ttname"><a href="../../d9/d8e/cpp_types_8h.html#a807200f3771dbd2cb2e4be52c5f2518c">DMat</a></div><div class="ttdeci">typename Eigen::Matrix&lt; T, Eigen::Dynamic, Eigen::Dynamic &gt; DMat</div><div class="ttdef"><b>Definition:</b> <a href="../../d9/d8e/cpp_types_8h_source.html#l00106">cppTypes.h:106</a></div></div>
<div class="ttc" id="cpp_types_8h_html_a6f07728c866b15ff0676a9b2ebfd2ba6"><div class="ttname"><a href="../../d9/d8e/cpp_types_8h.html#a6f07728c866b15ff0676a9b2ebfd2ba6">Vec3</a></div><div class="ttdeci">typename Eigen::Matrix&lt; T, 3, 1 &gt; Vec3</div><div class="ttdef"><b>Definition:</b> <a href="../../d9/d8e/cpp_types_8h_source.html#l00026">cppTypes.h:26</a></div></div>
<div class="ttc" id="namespacestd_html"><div class="ttname"><a href="../../d8/dcc/namespacestd.html">std</a></div></div>
<div class="ttc" id="class_floating_base_model_html_a79fe9f46e0e6617b6e68a6f267f877dc"><div class="ttname"><a href="../../d6/db1/class_floating_base_model.html#a79fe9f46e0e6617b6e68a6f267f877dc">FloatingBaseModel::getLinearAcceleration</a></div><div class="ttdeci">Vec3&lt; T &gt; getLinearAcceleration(const int link_idx, const Vec3&lt; T &gt; &amp;point)</div><div class="ttdef"><b>Definition:</b> <a href="../../da/de7/_floating_base_model_8cpp_source.html#l00698">FloatingBaseModel.cpp:698</a></div></div>
<div class="ttc" id="struct_f_b_model_state_derivative_html_a8b7b6d70f1b1268f02e1b291dad26c5d"><div class="ttname"><a href="../../de/dce/struct_f_b_model_state_derivative.html#a8b7b6d70f1b1268f02e1b291dad26c5d">FBModelStateDerivative::dBodyVelocity</a></div><div class="ttdeci">SVec&lt; T &gt; dBodyVelocity</div><div class="ttdef"><b>Definition:</b> <a href="../../d5/d9a/_floating_base_model_8h_source.html#l00060">FloatingBaseModel.h:60</a></div></div>
<div class="ttc" id="namespacespatial_html_aba5e712bd3844d2e6a57dcb071f3f0a5"><div class="ttname"><a href="../../de/d1a/namespacespatial.html#aba5e712bd3844d2e6a57dcb071f3f0a5">spatial::translationFromSXform</a></div><div class="ttdeci">auto translationFromSXform(const Eigen::MatrixBase&lt; T &gt; &amp;X)</div><div class="ttdef"><b>Definition:</b> <a href="../../d9/dc8/spatial_8h_source.html#l00168">spatial.h:168</a></div></div>
<div class="ttc" id="_utilities__print_8h_html"><div class="ttname"><a href="../../d3/d68/_utilities__print_8h.html">Utilities_print.h</a></div></div>
<div class="ttc" id="namespacespatial_html_a13df9ec1359ecd4150251eaae0895198"><div class="ttname"><a href="../../de/d1a/namespacespatial.html#a13df9ec1359ecd4150251eaae0895198">spatial::spatialToLinearAcceleration</a></div><div class="ttdeci">auto spatialToLinearAcceleration(const Eigen::MatrixBase&lt; T &gt; &amp;a, const Eigen::MatrixBase&lt; T2 &gt; &amp;v)</div><div class="ttdef"><b>Definition:</b> <a href="../../d9/dc8/spatial_8h_source.html#l00289">spatial.h:289</a></div></div>
<div class="ttc" id="namespacespatial_html_ab521121fc7af2392144a78003c100a10"><div class="ttname"><a href="../../de/d1a/namespacespatial.html#ab521121fc7af2392144a78003c100a10">spatial::sXFormPoint</a></div><div class="ttdeci">auto sXFormPoint(const Eigen::MatrixBase&lt; T &gt; &amp;X, const Eigen::MatrixBase&lt; T2 &gt; &amp;p)</div><div class="ttdef"><b>Definition:</b> <a href="../../d9/dc8/spatial_8h_source.html#l00329">spatial.h:329</a></div></div>
<div class="ttc" id="namespaceori_html_a02c20a1b1d35988c657aa1e219d609d5"><div class="ttname"><a href="../../da/d9b/namespaceori.html#a02c20a1b1d35988c657aa1e219d609d5">ori::CoordinateAxis</a></div><div class="ttdeci">CoordinateAxis</div><div class="ttdef"><b>Definition:</b> <a href="../../d4/dfb/orientation__tools_8h_source.html#l00032">orientation_tools.h:32</a></div></div>
<div class="ttc" id="cpp_types_8h_html_a190e396ab8cb0eb974ad2a5195edcb24"><div class="ttname"><a href="../../d9/d8e/cpp_types_8h.html#a190e396ab8cb0eb974ad2a5195edcb24">D3Mat</a></div><div class="ttdeci">typename Eigen::Matrix&lt; T, 3, Eigen::Dynamic &gt; D3Mat</div><div class="ttdef"><b>Definition:</b> <a href="../../d9/d8e/cpp_types_8h_source.html#l00114">cppTypes.h:114</a></div></div>
<div class="ttc" id="cpp_types_8h_html_a3d70f8eb9cf1855ca989f887a5543acc"><div class="ttname"><a href="../../d9/d8e/cpp_types_8h.html#a3d70f8eb9cf1855ca989f887a5543acc">SVec</a></div><div class="ttdeci">typename Eigen::Matrix&lt; T, 6, 1 &gt; SVec</div><div class="ttdef"><b>Definition:</b> <a href="../../d9/d8e/cpp_types_8h_source.html#l00062">cppTypes.h:62</a></div></div>
<div class="ttc" id="class_floating_base_model_html_abd6c4bde8153cace7d96761c7d6c56cc"><div class="ttname"><a href="../../d6/db1/class_floating_base_model.html#abd6c4bde8153cace7d96761c7d6c56cc">FloatingBaseModel::biasAccelerations</a></div><div class="ttdeci">void biasAccelerations()</div><div class="ttdef"><b>Definition:</b> <a href="../../da/de7/_floating_base_model_8cpp_source.html#l00590">FloatingBaseModel.cpp:590</a></div></div>
<div class="ttc" id="class_floating_base_model_html_afca11b3f8d117a20990d309666e1a8f3"><div class="ttname"><a href="../../d6/db1/class_floating_base_model.html#afca11b3f8d117a20990d309666e1a8f3">FloatingBaseModel::contactJacobians</a></div><div class="ttdeci">void contactJacobians()</div><div class="ttdef"><b>Definition:</b> <a href="../../da/de7/_floating_base_model_8cpp_source.html#l00548">FloatingBaseModel.cpp:548</a></div></div>
<div class="ttc" id="class_floating_base_model_html_a76cd0fcd431c77e002d8e111b2880cbe"><div class="ttname"><a href="../../d6/db1/class_floating_base_model.html#a76cd0fcd431c77e002d8e111b2880cbe">FloatingBaseModel::totalRotorMass</a></div><div class="ttdeci">T totalRotorMass()</div><div class="ttdef"><b>Definition:</b> <a href="../../da/de7/_floating_base_model_8cpp_source.html#l00465">FloatingBaseModel.cpp:465</a></div></div>
<div class="ttc" id="class_floating_base_model_html_a701f7e9f31622845a4a49b30b0846d55"><div class="ttname"><a href="../../d6/db1/class_floating_base_model.html#a701f7e9f31622845a4a49b30b0846d55">FloatingBaseModel::getPosition</a></div><div class="ttdeci">Vec3&lt; T &gt; getPosition(const int link_idx, const Vec3&lt; T &gt; &amp;local_pos)</div><div class="ttdef"><b>Definition:</b> <a href="../../da/de7/_floating_base_model_8cpp_source.html#l00689">FloatingBaseModel.cpp:689</a></div></div>
<div class="ttc" id="namespacespatial_html_ae4a2145f3028087511b4f17c0152ec83"><div class="ttname"><a href="../../de/d1a/namespacespatial.html#ae4a2145f3028087511b4f17c0152ec83">spatial::createSXform</a></div><div class="ttdeci">auto createSXform(const Eigen::MatrixBase&lt; T &gt; &amp;R, const Eigen::MatrixBase&lt; T2 &gt; &amp;r)</div><div class="ttdef"><b>Definition:</b> <a href="../../d9/dc8/spatial_8h_source.html#l00140">spatial.h:140</a></div></div>
<div class="ttc" id="class_spatial_inertia_html"><div class="ttname"><a href="../../d6/d81/class_spatial_inertia.html">SpatialInertia</a></div><div class="ttdef"><b>Definition:</b> <a href="../../d4/d7d/_spatial_inertia_8h_source.html#l00025">SpatialInertia.h:25</a></div></div>
<div class="ttc" id="class_floating_base_model_html_a8d1ea8345adf3034553a1a7f6646bd5e"><div class="ttname"><a href="../../d6/db1/class_floating_base_model.html#a8d1ea8345adf3034553a1a7f6646bd5e">FloatingBaseModel::forwardAccelerationKinematics</a></div><div class="ttdeci">void forwardAccelerationKinematics()</div><div class="ttdef"><b>Definition:</b> <a href="../../da/de7/_floating_base_model_8cpp_source.html#l00813">FloatingBaseModel.cpp:813</a></div></div>
<div class="ttc" id="class_floating_base_model_html_adcff6806f39aedf95a3723fc10c6144e"><div class="ttname"><a href="../../d6/db1/class_floating_base_model.html#adcff6806f39aedf95a3723fc10c6144e">FloatingBaseModel::addBody</a></div><div class="ttdeci">int addBody(const SpatialInertia&lt; T &gt; &amp;inertia, const SpatialInertia&lt; T &gt; &amp;rotorInertia, T gearRatio, int parent, JointType jointType, CoordinateAxis jointAxis, const Mat6&lt; T &gt; &amp;Xtree, const Mat6&lt; T &gt; &amp;Xrot)</div><div class="ttdef"><b>Definition:</b> <a href="../../da/de7/_floating_base_model_8cpp_source.html#l00392">FloatingBaseModel.cpp:392</a></div></div>
<div class="ttc" id="class_floating_base_model_html_a4ecf73ee1e17247d25f80b33c07da0fd"><div class="ttname"><a href="../../d6/db1/class_floating_base_model.html#a4ecf73ee1e17247d25f80b33c07da0fd">FloatingBaseModel::compositeInertias</a></div><div class="ttdeci">void compositeInertias()</div><div class="ttdef"><b>Definition:</b> <a href="../../da/de7/_floating_base_model_8cpp_source.html#l00753">FloatingBaseModel.cpp:753</a></div></div>
<div class="ttc" id="namespacespatial_html_a49f73699d3c57d07835e8ab61af3f34a"><div class="ttname"><a href="../../de/d1a/namespacespatial.html#a49f73699d3c57d07835e8ab61af3f34a">spatial::motionCrossProduct</a></div><div class="ttdeci">auto motionCrossProduct(const Eigen::MatrixBase&lt; T &gt; &amp;a, const Eigen::MatrixBase&lt; T &gt; &amp;b)</div><div class="ttdef"><b>Definition:</b> <a href="../../d9/dc8/spatial_8h_source.html#l00073">spatial.h:73</a></div></div>
<div class="ttc" id="class_floating_base_model_html_a4c4c27d3bc26c3ad1a3306d16b57f085"><div class="ttname"><a href="../../d6/db1/class_floating_base_model.html#a4c4c27d3bc26c3ad1a3306d16b57f085">FloatingBaseModel::forwardKinematics</a></div><div class="ttdeci">void forwardKinematics()</div><div class="ttdef"><b>Definition:</b> <a href="../../da/de7/_floating_base_model_8cpp_source.html#l00479">FloatingBaseModel.cpp:479</a></div></div>
<div class="ttc" id="namespacespatial_html_acc851583f244f19d9ea07490e9328b51"><div class="ttname"><a href="../../de/d1a/namespacespatial.html#acc851583f244f19d9ea07490e9328b51">spatial::JointType</a></div><div class="ttdeci">JointType</div><div class="ttdef"><b>Definition:</b> <a href="../../d9/dc8/spatial_8h_source.html#l00021">spatial.h:21</a></div></div>
<div class="ttc" id="struct_f_b_model_state_derivative_html"><div class="ttname"><a href="../../de/dce/struct_f_b_model_state_derivative.html">FBModelStateDerivative</a></div><div class="ttdef"><b>Definition:</b> <a href="../../d5/d9a/_floating_base_model_8h_source.html#l00057">FloatingBaseModel.h:57</a></div></div>
<div class="ttc" id="class_floating_base_model_html_aad1c033e1d94d42ee028accaf6a48b1e"><div class="ttname"><a href="../../d6/db1/class_floating_base_model.html#aad1c033e1d94d42ee028accaf6a48b1e">FloatingBaseModel::updateArticulatedBodies</a></div><div class="ttdeci">void updateArticulatedBodies()</div><div class="ttdef"><b>Definition:</b> <a href="../../da/de7/_floating_base_model_8cpp_source.html#l00142">FloatingBaseModel.cpp:142</a></div></div>
<div class="ttc" id="class_floating_base_model_html_a759338421e6adbe3a0a69e42271d43d4"><div class="ttname"><a href="../../d6/db1/class_floating_base_model.html#a759338421e6adbe3a0a69e42271d43d4">FloatingBaseModel::invContactInertia</a></div><div class="ttdeci">DMat&lt; T &gt; invContactInertia(const int gc_index, const D6Mat&lt; T &gt; &amp;force_directions)</div><div class="ttdef"><b>Definition:</b> <a href="../../da/de7/_floating_base_model_8cpp_source.html#l01065">FloatingBaseModel.cpp:1065</a></div></div>
<div class="ttc" id="class_floating_base_model_html_a8caeb9858d5b992b6e9f9d525be81376"><div class="ttname"><a href="../../d6/db1/class_floating_base_model.html#a8caeb9858d5b992b6e9f9d525be81376">FloatingBaseModel::resizeSystemMatricies</a></div><div class="ttdeci">void resizeSystemMatricies()</div><div class="ttdef"><b>Definition:</b> <a href="../../da/de7/_floating_base_model_8cpp_source.html#l00242">FloatingBaseModel.cpp:242</a></div></div>
<div class="ttc" id="class_floating_base_model_html_a5247d4939f30a11b61bebdaa3e55ce5b"><div class="ttname"><a href="../../d6/db1/class_floating_base_model.html#a5247d4939f30a11b61bebdaa3e55ce5b">FloatingBaseModel::getAngularAcceleration</a></div><div class="ttdeci">Vec3&lt; T &gt; getAngularAcceleration(const int link_idx)</div><div class="ttdef"><b>Definition:</b> <a href="../../da/de7/_floating_base_model_8cpp_source.html#l00740">FloatingBaseModel.cpp:740</a></div></div>
<div class="ttc" id="namespacespatial_html"><div class="ttname"><a href="../../de/d1a/namespacespatial.html">spatial</a></div><div class="ttdef"><b>Definition:</b> <a href="../../d9/dc8/spatial_8h_source.html#l00019">spatial.h:19</a></div></div>
<div class="ttc" id="class_floating_base_model_html"><div class="ttname"><a href="../../d6/db1/class_floating_base_model.html">FloatingBaseModel&lt; double &gt;</a></div></div>
<div class="ttc" id="class_floating_base_model_html_a1edbea0371cffe8692a83ae5a9396e41"><div class="ttname"><a href="../../d6/db1/class_floating_base_model.html#a1edbea0371cffe8692a83ae5a9396e41">FloatingBaseModel::applyTestForce</a></div><div class="ttdeci">T applyTestForce(const int gc_index, const Vec3&lt; T &gt; &amp;force_ics_at_contact, FBModelStateDerivative&lt; T &gt; &amp;dstate_out)</div><div class="ttdef"><b>Definition:</b> <a href="../../da/de7/_floating_base_model_8cpp_source.html#l00962">FloatingBaseModel.cpp:962</a></div></div>
<div class="ttc" id="orientation__tools_8h_html"><div class="ttname"><a href="../../d4/dfb/orientation__tools_8h.html">orientation_tools.h</a></div><div class="ttdoc">Utility functions for 3D rotations. </div></div>
<div class="ttc" id="struct_f_b_model_state_derivative_html_af32a2041b72e6b30ed1c8a027d340fd2"><div class="ttname"><a href="../../de/dce/struct_f_b_model_state_derivative.html#af32a2041b72e6b30ed1c8a027d340fd2">FBModelStateDerivative::qdd</a></div><div class="ttdeci">DVec&lt; T &gt; qdd</div><div class="ttdef"><b>Definition:</b> <a href="../../d5/d9a/_floating_base_model_8h_source.html#l00061">FloatingBaseModel.h:61</a></div></div>
<div class="ttc" id="cpp_types_8h_html_ae2e5a0a178a2c528d8aa939f85db7a3e"><div class="ttname"><a href="../../d9/d8e/cpp_types_8h.html#ae2e5a0a178a2c528d8aa939f85db7a3e">DVec</a></div><div class="ttdeci">typename Eigen::Matrix&lt; T, Eigen::Dynamic, 1 &gt; DVec</div><div class="ttdef"><b>Definition:</b> <a href="../../d9/d8e/cpp_types_8h_source.html#l00102">cppTypes.h:102</a></div></div>
<div class="ttc" id="class_floating_base_model_html_ac3e58feb2dceedb9739bc76bcaccbc98"><div class="ttname"><a href="../../d6/db1/class_floating_base_model.html#ac3e58feb2dceedb9739bc76bcaccbc98">FloatingBaseModel::totalNonRotorMass</a></div><div class="ttdeci">T totalNonRotorMass()</div><div class="ttdef"><b>Definition:</b> <a href="../../da/de7/_floating_base_model_8cpp_source.html#l00452">FloatingBaseModel.cpp:452</a></div></div>
<div class="ttc" id="namespaceori_html"><div class="ttname"><a href="../../da/d9b/namespaceori.html">ori</a></div><div class="ttdef"><b>Definition:</b> <a href="../../d4/dfb/orientation__tools_8h_source.html#l00028">orientation_tools.h:28</a></div></div>
<div class="ttc" id="namespaceori_html_af88fa91d686c646d41f9b747a6bc90f0"><div class="ttname"><a href="../../da/d9b/namespaceori.html#af88fa91d686c646d41f9b747a6bc90f0">ori::quaternionToRotationMatrix</a></div><div class="ttdeci">Mat3&lt; typename T::Scalar &gt; quaternionToRotationMatrix(const Eigen::MatrixBase&lt; T &gt; &amp;q)</div><div class="ttdef"><b>Definition:</b> <a href="../../d4/dfb/orientation__tools_8h_source.html#l00160">orientation_tools.h:160</a></div></div>
<div class="ttc" id="class_floating_base_model_html_aff2cddc3077b282fe3504ecd28bc08cb"><div class="ttname"><a href="../../d6/db1/class_floating_base_model.html#aff2cddc3077b282fe3504ecd28bc08cb">FloatingBaseModel::addGroundContactBoxPoints</a></div><div class="ttdeci">void addGroundContactBoxPoints(int bodyId, const Vec3&lt; T &gt; &amp;dims)</div><div class="ttdef"><b>Definition:</b> <a href="../../da/de7/_floating_base_model_8cpp_source.html#l00360">FloatingBaseModel.cpp:360</a></div></div>
<div class="ttc" id="namespaceori_html_a02c20a1b1d35988c657aa1e219d609d5a02129bb861061d1a052c592e2dc6b383"><div class="ttname"><a href="../../da/d9b/namespaceori.html#a02c20a1b1d35988c657aa1e219d609d5a02129bb861061d1a052c592e2dc6b383">ori::CoordinateAxis::X</a></div></div>
<div class="ttc" id="cpp_types_8h_html_ad7673cd2189f78a761c3a51d9c1314b1"><div class="ttname"><a href="../../d9/d8e/cpp_types_8h.html#ad7673cd2189f78a761c3a51d9c1314b1">MassProperties</a></div><div class="ttdeci">typename Eigen::Matrix&lt; T, 10, 1 &gt; MassProperties</div><div class="ttdef"><b>Definition:</b> <a href="../../d9/d8e/cpp_types_8h_source.html#l00098">cppTypes.h:98</a></div></div>
<div class="ttc" id="class_floating_base_model_html_ac4097f466bc8a392a88273e33c17bb50"><div class="ttname"><a href="../../d6/db1/class_floating_base_model.html#ac4097f466bc8a392a88273e33c17bb50">FloatingBaseModel::getLinearVelocity</a></div><div class="ttdeci">Vec3&lt; T &gt; getLinearVelocity(const int link_idx, const Vec3&lt; T &gt; &amp;point)</div><div class="ttdef"><b>Definition:</b> <a href="../../da/de7/_floating_base_model_8cpp_source.html#l00714">FloatingBaseModel.cpp:714</a></div></div>
<div class="ttc" id="class_floating_base_model_html_aee861a77ddcefb97d62f0f535ee97508"><div class="ttname"><a href="../../d6/db1/class_floating_base_model.html#aee861a77ddcefb97d62f0f535ee97508">FloatingBaseModel::addDynamicsVars</a></div><div class="ttdeci">void addDynamicsVars(int count)</div><div class="ttdef"><b>Definition:</b> <a href="../../da/de7/_floating_base_model_8cpp_source.html#l00185">FloatingBaseModel.cpp:185</a></div></div>
<div class="ttc" id="class_floating_base_model_html_a73d58fada538f7d74ad94c48e5024b78"><div class="ttname"><a href="../../d6/db1/class_floating_base_model.html#a73d58fada538f7d74ad94c48e5024b78">FloatingBaseModel::massMatrix</a></div><div class="ttdeci">DMat&lt; T &gt; massMatrix()</div><div class="ttdef"><b>Definition:</b> <a href="../../da/de7/_floating_base_model_8cpp_source.html#l00778">FloatingBaseModel.cpp:778</a></div></div>
<div class="ttc" id="class_floating_base_model_html_ae79c032f47bdee3795e4707429a27885"><div class="ttname"><a href="../../d6/db1/class_floating_base_model.html#ae79c032f47bdee3795e4707429a27885">FloatingBaseModel::generalizedGravityForce</a></div><div class="ttdeci">DVec&lt; T &gt; generalizedGravityForce()</div><div class="ttdef"><b>Definition:</b> <a href="../../da/de7/_floating_base_model_8cpp_source.html#l00610">FloatingBaseModel.cpp:610</a></div></div>
<div class="ttc" id="cpp_types_8h_html_a0b83d93a3c74d282558436e26c056896"><div class="ttname"><a href="../../d9/d8e/cpp_types_8h.html#a0b83d93a3c74d282558436e26c056896">RotMat</a></div><div class="ttdeci">typename Eigen::Matrix&lt; T, 3, 3 &gt; RotMat</div><div class="ttdef"><b>Definition:</b> <a href="../../d9/d8e/cpp_types_8h_source.html#l00018">cppTypes.h:18</a></div></div>
<div class="ttc" id="namespacespatial_html_abdc3cd991206414d80d9f89be27e3633"><div class="ttname"><a href="../../de/d1a/namespacespatial.html#abdc3cd991206414d80d9f89be27e3633">spatial::rotationFromSXform</a></div><div class="ttdeci">auto rotationFromSXform(const Eigen::MatrixBase&lt; T &gt; &amp;X)</div><div class="ttdef"><b>Definition:</b> <a href="../../d9/dc8/spatial_8h_source.html#l00157">spatial.h:157</a></div></div>
<div class="ttc" id="_floating_base_model_8h_html"><div class="ttname"><a href="../../d5/d9a/_floating_base_model_8h.html">FloatingBaseModel.h</a></div><div class="ttdoc">Implementation of Rigid Body Floating Base model data structure. </div></div>
<div class="ttc" id="class_floating_base_model_html_ab3dfc6605561bee26b4f7a893e44c78d"><div class="ttname"><a href="../../d6/db1/class_floating_base_model.html#ab3dfc6605561bee26b4f7a893e44c78d">FloatingBaseModel::addBase</a></div><div class="ttdeci">void addBase(const SpatialInertia&lt; T &gt; &amp;inertia)</div><div class="ttdef"><b>Definition:</b> <a href="../../da/de7/_floating_base_model_8cpp_source.html#l00267">FloatingBaseModel.cpp:267</a></div></div>
<div class="ttc" id="class_floating_base_model_html_ae7c9130e9835487677c82edd2918f4e0"><div class="ttname"><a href="../../d6/db1/class_floating_base_model.html#ae7c9130e9835487677c82edd2918f4e0">FloatingBaseModel::udpateQddEffects</a></div><div class="ttdeci">void udpateQddEffects()</div><div class="ttdef"><b>Definition:</b> <a href="../../da/de7/_floating_base_model_8cpp_source.html#l00098">FloatingBaseModel.cpp:98</a></div></div>
<div class="ttc" id="test__casadi_8cpp_html_ae20d7fba59865a21504b65cdb44cbfe5"><div class="ttname"><a href="../../dc/dd7/test__casadi_8cpp.html#ae20d7fba59865a21504b65cdb44cbfe5">f</a></div><div class="ttdeci">MX f(const MX &amp;x, const MX &amp;u)</div><div class="ttdef"><b>Definition:</b> <a href="../../dc/dd7/test__casadi_8cpp_source.html#l00202">test_casadi.cpp:202</a></div></div>
<div class="ttc" id="namespacespatial_html_af37757ee8ad4510df5478202a62c9502"><div class="ttname"><a href="../../de/d1a/namespacespatial.html#af37757ee8ad4510df5478202a62c9502">spatial::invertSXform</a></div><div class="ttdeci">auto invertSXform(const Eigen::MatrixBase&lt; T &gt; &amp;X)</div><div class="ttdef"><b>Definition:</b> <a href="../../d9/dc8/spatial_8h_source.html#l00181">spatial.h:181</a></div></div>
</div><!-- fragment --></div><!-- contents -->
</div><!-- doc-content -->
<!-- start footer part -->
<div id="nav-path" class="navpath"><!-- id is needed for treeview function! -->
  <ul>
    <li class="navelem"><a class="el" href="../../dir_43e0a1f539e00dcfa1a6bc4d4fee4fc2.html">home</a></li><li class="navelem"><a class="el" href="../../dir_61bfb0b3cdda81b1b0355ba34eb3b1eb.html">dhkim</a></li><li class="navelem"><a class="el" href="../../dir_adee5b101e079090017bb4e91c628ead.html">Public_Repository</a></li><li class="navelem"><a class="el" href="../../dir_7fc07a7b97001e7b1d18e11da761e2de.html">Cheetah-Software</a></li><li class="navelem"><a class="el" href="../../dir_860379c26524a894b3cf97b5b1ba5bdb.html">common</a></li><li class="navelem"><a class="el" href="../../dir_054bbb67b14bc4d9ad4b8f9fd2c17d22.html">src</a></li><li class="navelem"><a class="el" href="../../dir_15eb938ed49f19e0fef569b69def6abf.html">Dynamics</a></li><li class="navelem"><a class="el" href="../../da/de7/_floating_base_model_8cpp.html">FloatingBaseModel.cpp</a></li>
    <li class="footer">Generated by
    <a href="http://www.doxygen.org/index.html">
    <img class="footer" src="../../doxygen.png" alt="doxygen"/></a> 1.8.11 </li>
  </ul>
</div>
</body>
</html>
